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1.  Introduction 


1.1  Legacy  Human  Steering  Model 

Human  steering  models  have  been  used  by  the  automotive  industry  as  part  of  their  vehicle 
dynamics  modeling  efforts.  The  U.S.  Army  Tank  Automotive  Command  contracted  with  the 
University  of  Michigan  Transportation  Research  Institute  to  modify  one  of  the  industrial  models 
to  work  with  military  vehicles.  The  steering  model  was  modified  to  work  with  wheeled  and 
tracked  military  vehicles.  We  could  model  conventional  front  wheel  steering,  front  and  rear 
wheel  steering,  and  skid  steering  by  changing  input  parameters  in  the  model.  At  the  conclusion 
of  the  contract,  a  report  entitled  “Development  of  DriverWehicle  Steering  Interaction  Models  for 
Dynamic  Analysis”  (1)  was  written  about  the  new  human  steering  model.  The  existing  or 
“legacy”  human  steering  model  that  served  as  the  basis  for  the  current  study  was  taken  from 
reference  (1). 

1.2  Software  Architecture 

This  study  added  a  new  sensor  model  to  the  steering  model  and  linked  the  modified  steering 
model  to  a  vehicle  dynamics  simulation  in  the  dynamics  analysis  and  design  system  (DADS^). 
The  original  steering  model  contained  a  simple  two-dimensional  (2-D)  sensor  model  that  did  not 
take  into  account  the  pitch  and  yaw  of  the  vehicle  or  height  of  objects  in  the  virtual  environment. 
The  new  sensor  model  is  three-dimensional  (3-D)  and  completely  coupled  to  the  vehicle 
dynamics  model.  Figure  1  shows  the  basic  process  flow  of  the  new  integrated  model.  The 
external  vehicle  dynamics  model  was  developed  within  DADS  as  a  complex  multi-body 
simulation.  The  desired  path  is  input  as  a  series  of  3-D  obstacles  that  border  the  route,  similar  to 
concrete  “Jersey”  barriers.  The  sensor  model  measures  the  distance  from  the  vehicle  to  the 
surrounding  obstacles.  Based  on  the  range  map  developed  by  the  sensor,  the  steering  model 
plans  or  previews  a  path  for  the  vehicle.  The  steering  model  tries  to  maintain  the  vehicle 
equidistant  from  obstacles  to  the  right  and  left.  The  steering  model  contains  a  simplified  internal 
model  of  the  vehicle’s  dynamics.  The  internal  model  is  a  linearized,  2-D  model.  The  internal 
model  represents  the  human  driver’s  perception  of  how  the  vehicle  behaves  and  is  used  to  predict 
the  vehicle’s  state  at  some  time  in  the  future.  The  steering  model  takes  as  input  the  current  state 
of  the  vehicle,  previewed  path,  and  the  predicted  vehicle  state  during  a  preview  interval.  The 
steering  model  determines  the  difference  between  the  previewed  and  the  predicted  path  of  the 
vehicle  during  the  preview  interval  called  the  “path  error”.  The  steering  model  calculates  a 
steering  command  that  minimizes  the  “path  error”.  The  steering  command  is  passed  to  the 
external  vehicle  model  and  produces  control  forces  that  change  the  vehicle’s  state. 


'dads™  is  a  trademark  of  LMS  (not  an  acronym). 
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Figure  1.  Steering  model  process  flow. 

A  complete  listing  of  the  modified  human  steering  model  and  its  interface  to  DADS  is  given  in 
appendix  A.  The  eontrol  portion  of  the  human  steering  model  has  not  been  signifieantly 
modified  exeept  for  eomments  in  the  eode.  The  comments  have  been  modified  to  bring  them  in 
line  with  the  derivation  of  the  control  algorithm  given  in  this  report.  The  subroutines  taken 
direetly  from  the  reference  (1)  are  Avoid  Obstacles,  CALCRS,  CALCTH,  CHECKRTH, 
GMPRD,  NEWDRIVER,  and  TRANS.  The  other  subroutines  are  either  new  or  are  major 
revisions  of  subroutines  presented  in  referenee  (1). 


2.  Human  Steering  Simulation 


2,1  The  Steering  Control  Loop 

Eigure  2  shows  a  block  diagram  of  the  control  algorithm  used  in  the  steering  subroutines 
“NEWDRIVER”.  The  parameters  used  in  the  steering  algorithm  are  initialized  by 
“HUMAN_STEERING”  whieh  then  ealls  “NEWDRIVER”.  The  algorithm  in  “NEWDRIVER” 
calculates  a  steering  eontrol  signal  over  a  time  interval  called  the  preview  time.  The  control 
signal  is  calculated  to  minimize  the  difference  or  error  between  the  planned  or  previewed  path 
and  the  vehicle’s  aetual  path.  The  steering  control  signal  is  delayed  by  a  fix  amount  of  time 
before  it  is  sent  to  the  external  vehicle  dynamics  model.  The  fixed  delay  represents  the  human 
driver’s  neuromuscular  delay  in  generating  the  command  signal,  i.e.,  turning  the  steering  wheel. 
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Figure  2.  Control  algorithm  block  diagram. 


The  course  through  the  algorithm  starts  at  the  left  with  the  previewed  vehicle’s  path  information 
received  from  the  vehicle’s  sensors.  The  previewed  path  is  subtracted  from  the  path  predicted  by 
the  driver.  The  difference  is  the  error  to  be  minimized  by  the  control  algorithm.  The  error  is 
acted  upon  by  the  gain  to  produce  a  revised  steering  signal.  The  revision  is  summed  with  the 
current  steering  signal.  The  new  steering  signal  is  delayed  by  the  neuromuscular  delay  to 
become  the  current  steering  signal.  The  current  steering  signal  is  sent  to  the  external  vehicle 
dynamics  model  where  it  produces  control  forces.  The  external  vehicle  dynamics  model 
calculates  the  effects  of  the  control  forces  on  the  vehicle  state.  The  current  steering  signal  is  also 
sent  backward  to  be  summed  with  the  steering  signal  update.  The  current  steering  signal  is  used 
by  the  driver  to  predict  the  vehicle  path  during  the  preview  interval.  The  current  vehicle  state  is 
used  in  a  separate  calculation  by  the  driver  to  predict  the  vehicle’s  path  during  the  preview 
interval.  These  two  predictions  of  the  path  are  summed  to  produce  the  current  predicted  path 
during  the  preview  interval.  The  current  predicted  path  is  fed  back  and  summed  with  the  current 
previewed  path.  Finally,  the  vehicle  state  is  sent  to  the  sensor  model  where  it  is  used  to  revise 
the  current  previewed  path  information. 


2,2  Derivation  of  the  Steering  Algorithm 

Reference  (i)  gives  a  derivation  of  the  steering  algorithm,  but  the  listing  of  the  code  in  appendix 
A  shows  a  somewhat  different  algorithm.  The  algorithm  that  was  actually  used  to  calculate  the 
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steering  signal  is  derived  in  this  report.  In  the  derivation  of  the  eontrol  algorithm,  the  system  is 
assumed  to  be  linear.  The  equation  motion  of  the  vehiele  is 


x  =  F  x+gu 

y  =  X 


in  whieh 

X  is  the  vehiele  state  vector, 

X  is  the  vector  containing  the  time  derivative  of  the  vehicle  state, 
g  is  the  control  coefficient  vector. 


U  is  the  scalar  steering  command,  and 
F  is  the  4  by  4  system  matrix. 

Note:  Parameters  in  italics  are  vectors,  parameters  in  bold  and  italics  are  matrices,  and 
parameters  written  in  New  Gothic  MT  font  are  scalars. 

The  vehicle’s  state  vector  x  and  the  fundamental  matrix  F  are  given  by  the  following: 


y 


V 

r 


V 


0  1 

0  -2(Q+C,,)/mU 
0  2(bQ -qC„,  )/IU 
0  0 


0  u 

2(bCQf  -  aCar  )/fnU  -  U  0 
1  0 


In  equations  2  and  3,  the  parameters  are  defined  as  follows: 

Y  =  vehicle’s  lateral  displacement 

V  =  vehicle’s  lateral  velocity 
\|/  =  vehicle’s  yaw  angle 

r  =  vehicle’s  yaw  rate 

U  =  vehicle’s  forward  velocity  (assumed  to  be  constant) 
m  =  vehicle’s  mass 

I  =  vehicle’s  moment  of  inertia  about  the  vertical  axis 

a  =  distance  from  the  vehicle’s  center  of  gravity  (eg)  to  the  front  axle  centerline 
b  =  distance  from  the  vehicle’s  eg  to  the  rear  axle  centerline 


(1) 


(2) 


(3) 
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Caf  =  front  tire  cornering  stiffness 
Car  =  the  rear  tire  cornering  stiffness 
ff  =  the  front  wheel  steering  angle 
fr  =  the  rear  wheel  steering  angle 
k  =  the  proportion  of  ff 

The  vehicle’s  dynamic  transition  vector  g  is  given  by  the  following: 


g  = 


0 


a[2(q  +kQ)/m]  +  B/m 
)/l]  +  D/I 

0 


C[2(Q+kbQ 


(4) 


In  equation  4,  the  A,  B,  C,  and  D  parameters  are  used  to  model  different  types  of  steering  (1). 
When  A  and  C  are  set  to  1.0  and  B,  D,  and  k  are  set  to  0.0,  the  standard  front-wheel-only 
steering  is  modeled.  If  front  and  rear  steering  is  modeled,  A,  B,  C,  and  D  remain  the  same  but  k 
assumes  some  value  between  0  and  1.  In  reference  (i),  tests  were  conducted  with  k  =  0.75. 
When  A,  B,  and  C  are  set  to  0.0  and  D  is  set  to  I.O,  skid  steering  is  modeled.  In  the  modeling  of 
a  tracked  vehicle,  Caf  and  Car  are  interpreted  as  an  equivalent  lateral  force  generated  by  track 
elements  because  of  side  slip. 

The  first  part  of  equation  1 ,  the  equations  of  motion,  can  be  put  in  the  following  form: 

X  =  A- x+  /?(t) 

with  the  initial  condition  given  by  the  following: 

x(0)  =  Xo 

in  which  xo  is  some  known  initial  position. 

In  equation  5, 

A  =  F 

h(t)  =  g-u{-^) 

The  fundamental  matrix  ^  is  defined  sothat  (2) 

^  =  A  -  (j) 

^(0)  =  / 


(5) 

(6) 


Laplace  transforms  will  be  used  to  solve  equation  7  for  ^ .  Taking  the  Laplace  transform  of  each 
side  of  equation  7  yields  the  following  (5): 


=4a-(^] 


(8) 
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When  the  initial  eondition  from  equation  7  is  used  in  equation  8,  it  can  be  rewritten  as  follows: 

-  (z>(0)  =  -I  =  A-i[(l)\  (9) 


in  which  s  is  some  positive  real  number. 

Solving  equation  9  for  (f)  and  substituting  for^,  we  get  the  following: 

^  =  i-i-{A-Siy\=ri-{F-Sl)-^\  (10) 

In  equation  10,  f  is  the  inverse  Laplace  transform. 


Returning  to  equation  5  and  taking  the  Laplace  transform  of  both  sides  gives  the  following: 


i\pc\  =  f\A  ■  x+  /((t)] 
SX(s)-Xo  =A-Xq+H{s) 


in  which  H  is  the  Laplace  transform  of  h 
Solving  equation  1 1  for  x(s)  gives  the  following: 

X(s)  =  1-  (^  -  s/)-'  J-  Vo  + 1-  (^  -  s/)-'  J -//(s)  (12) 

Taking  the  inverse  Laplace  transform  of  each  side  of  equation  12  and  solving  forXgives  the 
following: 


-'[;^(s)]=r'l-(^-s/)-'J-xo+[-(^-s/)-'J-//(s)) 

x(t)  =  f  ■'  j-  -S/)“'  ]■  X  f  {-  [a  -  S/)“'  ]•  //(s)} 


(13) 


(14) 


With  equation  10,  the  first  left-hand  term  can  be  rewritten  as  follows: 

^■'{-(^-S/)”‘}-Vo  =^-Xo 

With  the  convolution  theorem  and  equation  10,  the  second  term  of  equation  13  can  be  written  as 

t 


r '  1- (^  -  s/)-‘ ]  w}=  j  <^(t  -  4)  ■  A(t  )d| 

0 

Using  equations  14  and  15  in  equation  13  and  using  the  definition  of  h  gives 

t  t 

^(+)  =  •  ^0  +  J  =  (;>o  •  ^0  +  u(^)cl^ 

0  0 

Solving  for  the  vehicle’s  lateral  displacement  j  gives  the  following: 


(15) 


(16) 


y  =  m  -x' 


(t)  =  - 


•Vn  + 


I 


(17) 
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In  equation  17,  is  the  constant  observer  vector 

=\l  0  0  O] 


The  error  J,  between  the  previewed  vehicle  path  /(p)  and  the  predicted  vehicle  path  can  be 
written  as  follows(i) 

■^={^  jJ{/(^)-v(Tiydp  (18) 

^  '^0 


The  minimum  path  error  with  respect  to  the  steering  control  u  satisfies 


6J 

du 


Using  equation  17  to  substitute  for  ^(p)  in  equation  19, 


(19) 


dJ 


d 

du 


1  f 


n2 


/(n)- 


m 


(z)-xo  +  j(Z>(t-^)-g-u(^)d^ 


dp 


=  0 


(20) 


Differentiating  equation  20, 

dJ_  2  I 
du  “(tp-to)J 

To 

We  expanded  equation  21  by  substituting  u(^)  =  Uq  +  Au(^) ,  in  which  Uq  is  the  starting  steering 
control  parameter  and  Au(^)  is  the  change  over  the  preview  interval: 


du 


r  (t 

Jf(p)  -m^ ■U^{^-^)-g-d^ 


+  (Uo+Au(^)) 


Vo 

ft 


-m 


j0(t-^)-g-d^ 


VO 


du 


V 

1 


+  (Au(^)j 


f(p)+[-m'^-(|)(t-^)- 

t 

j^(t-4)-g-d| 


Xo\  +  Uo 


J(^(t-0-^-d^ 

0 

j(2>(t-^)-g-d^ 

0 

I- 

J^(t-^)-g-d^ 


Vo 

ft 


Xol- 


-m 


fj<z>(t-^)-g-d^ 


Vo 


dp  =  0 


I 

j^(t-0-g-d^ 


(22) 


Vo 


dp  =  0 


Solving  equation  22  for  m(^)  , 
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(23) 


Am(4)  = 


'P 

+0 


0 


l2 


dri 


Equation  23  gives  the  basie  algorithm  for  determining  the  ehange  in  steering  eommand  that 
minimizes  the  error  between  the  desired  vehiele  lateral  displaeement  at  the  end  of  the  preview 
time  interval  and  the  predieted  lateral  displaeement.  The  equations  are  written  in  terms  of  the 
fundamental  matrix  (j).  Therefore,  the  next  seetion  will  be  developed  in  the  way  (j)  is  ealeulated, 


Expanding  the  fundamental  matrix  (/)  gives  the  following: 


0  = 


'21 


'31 


v^4iy 


'21 


'22 


'23 


'13 


'23 


'33 


V^24  A^43  j 


'14 


'24 


'34 


v^44y 


= 


0 

0 


oYoYo) 


=  [ei 


6^  0 
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(24) 


(25) 


Equation  25  is  the  initial  eondition  for  equation  24.  Eaeh  eolurnn  in  equation  24  is  an 
independent  solution  of  the  homogeneous  part  of  equation  5,  given  below  as  equation  26. 

x  =  A-  x  =  F  ■  X  (26) 


Eaeh  of  the  four  independent  solutions  represented  by  the  columns  in  equation  24  can  be  written 
as  follows: 


Xi  - FjjXj  +  F 12X2  +  F 13X3  +  F 14X4 
^2  =  F 2iX[  +  F 22X2  +  F 23X3  +  F 24X4 
X3  =  F31X1  +  F32X2  +  F33X3  +  F34X4 
X4  =  F 4iX[  +  F 42X2  +  F 43X3  +  F 44X4 


(27) 


With  the  definition  of  the  matrix  F  given  in  equation  3,  equation  27  can  be  rewritten  as  follows: 

Xl  =X2  +  UX4 

^2  =  [-  2  )/mU]x2  +  [2  (bC^,  -  )/mU  -  U]x3 

X3  =  [2  (bC„,  -  QC,f  )/IU]x2  +  [-  2  (a^C^f  -  b^C^,  )/lu]x3 

X4  =X3 
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A  numerical  integration  of  equation  28  is  performed  four  times  to  produce  the  four  independent 
solution  vectors  that  comprise  the  columns  of  equation  24.  With  equation  25,  the  first  numerical 
integration  initializes  (xi,  X2,  X3,  X4)  to  ei;  the  second  initializes  (xi,  X2,  X3,  X4)  to  ei;  the  third 
initializes  (xi,  X2,  X3,  X4)  to  ey,  and  the  fourth  initializes  (xi,  X2,  X3,  X4)  to  64. 

The  time  integral  of  the  fundamental  matrix  in  the  denominator  of  equation  23  is  also  calculated 
with  a  numerical  integration.  Once  the  fundamental  matrix  and  its  time  integral  have  been  found 
by  numerical  integration,  the  matrix  multiplications  shown  in  equation  23  are  performed.  The 
numerator  and  denominator  of  equation  23  are  formed  and  the  division  is  performed.  The  results 
of  equation  23  give  the  change  in  the  steering  command,  and  this  is  added  to  the  previous 
steering  command  to  give  the  steering  command  that  minimizes  the  path  error. 

The  elements  of  the  F  matrix  given  in  equation  3  are  calculated  in  subroutine  “TRANS”.  The 
elements  of  the  g  vector  are  also  calculated  in  “TRANS”.  “TRANS”  also  calculates  the  elements 
of  the  fundamental  matrix  (j)  using  equation  26  and  a  numerical  integration.  The  calculation  is 

performed  four  times — one  time  for  each  of  the  initial  conditions  given  in  equation  25.  The  time 
integration  of  the  fundamental  matrix 

0 

is  also  performed  in  “TRANS”.  The  calculation  of  the  fundamental  matrix  and  its  integral  is 
performed  over  a  set  number  of  time  intervals,  and  the  result  of  each  calculation  is  stored  in  an 
array.  The  array  containing  the  stored  fundamental  matrices  and  their  integrals  is  passed  back  to 
subroutine  “NEWDRIVER”. 

The  calculation  of  the  change  in  steering  command  given  in  equation  23  is  performed  in 
subroutine  “NEWDRIVER”.  We  perform  the  matrix  multiplication  in  equation  23  by  calling 
subroutine  “GMPRD”.  The  integrations  shown  in  the  numerator  and  denominator  are  again  done 
numerically.  Einally,  dividing  the  numerator  by  the  denominator  produces  the  change  in  steering 
command,  which  is  added  to  the  previous  steering  command  to  produce  the  current  steering 
command.  The  steering  command  is  passed  back  to  subroutine  “HUMAN  STEERING”  which 
in  turn  passes  the  command  back  to  “Interface_Steering”.  “Interface  Steering”  passes  the 
command  onto  the  vehicle  dynamics  in  the  DADS  model. 


3.  Three-Dimensional  Driving  Sensor  Model 


The  3-D  driving  sensor  model  provides  information  about  the  virtual  environment  to  the  steering 
model.  The  sensor  model  is  attached  to  the  vehicle  dynamics  model.  The  position  and 
orientation  of  the  vehicle  determine  the  position  and  orientation  of  the  sensor  within  the 
environment.  The  sensor  does  not  change  position  or  orientation  relative  to  the  vehicle,  although 


9 


the  code  could  be  easily  extended  to  model  that  type  of  sensor  head  motion.  The  sensor  model 
produces  a  range  map  of  the  environment  that  is  the  basis  of  the  planned  or  previewed  path  for 
the  vehicle. 

3.1  Definition  of  Three-Dimensional  Obstacles 

The  environment  for  the  driver  model  consists  of  3-D  objects.  The  objects  are  defined  by  a  set 
of  corner  points  and  surfaces.  Subsets  of  the  corner  points  are  used  to  identify  the  outer  surfaces 
of  the  objects.  Each  type  of  3-D  object  is  defined  only  once  but  can  appear  in  the  environment 
any  number  of  times.  A  unique  location  and  orientation  define  each  instance  of  the  object  type 
in  the  environment.  The  sensor  model  measures  the  distance  from  the  sensor  center  to  the 
surfaces  of  the  3-D  objects  in  the  environment.  The  range  is  measured  along  a  series  of  scan 
rays.  The  distance  to  the  closest  object  along  this  set  of  scan  rays  forms  the  range  map  of  the 
environment. 

The  scan  rays  are  generated  on  the  basis  of  information  read  (scanned)  by  subroutine 
“HUMAN  DRIVER”.  The  subroutine  reads  the  maximum  range  at  which  the  sensor  can  detect 
obstacles.  The  scanning  pattern  of  the  sensor  is  defined  in  terms  of  horizontal  and  vertical  scan 
parameters.  The  scan  angles  are  defined  in  terms  of  spherical  coordinates  centered  on  the  sensor. 
Phi  ((]))  is  the  horizontal  scan  angle  measured  counterclockwise  from  the  vehicle’s  X-axis.  Theta 
(0)  is  the  vertical  scan  angle  measured  downward  from  the  vehicle’s  Z-axis.  “HEIMAN  D RIVER” 
reads  the  maximum  scan  angle  to  the  left  and  right  of  the  X-axis  and  the  maximum  scan  angle 
above  and  below  the  horizontal  from  a  user-supplied  input  file.  “HEIMAN  DRIVER”  also  reads 
Delta_Theta  and  Delta  Phi  which  define  the  angular  steps  between  scan  rays.  The  scan  pattern 
parameters  are  passed  to  ‘Tnterface_Steermg”  which,  in  turn,  passes  them  to  “Get_Obstacles”. 

The  3-D  objects  are  initialized  in  subroutine  “Get_Obstacles”.  Eigures  3  and  4  show  the  process 
flow  for  defining  the  3-D  objects  and  placing  them  in  the  virtual  environment  surrounding  the 
vehicle.  The  information  is  read  only  once,  the  first  time  the  subroutine  is  called.  The  number 
of  object  types  is  read  first.  Eor  each  object  type,  the  number  of  comer  points  is  read.  Eor  each 
comer  point  in  an  object  type,  an  X,Y,Z  location  is  entered.  The  first  set  of  corners  is  assumed 
to  be  part  of  a  polygon  in  the  Z  =  0  plane.  The  comers  are  entered  counterclockwise  around  the 
polygon,  with  as  many  as  eight  points  describing  the  polygon.  The  second  set  of  corner  points 
forms  a  second  polygon  displaced  in  the  negative  Z  direction.  The  corner  points  of  the  second 
polygon  are  again  entered  in  a  counterclockwise  direction  in  a  plane  with  a  constant  negative  Z 
value. 
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Once  the  eorner  points  have  been  read,  the  center  of  mass  (CM)  of  the  points  is  ealculated.  For 
the  CM  ealculation,  all  the  points  are  assigned  a  mass  of  one.  The  loeations  of  the  eorner  points 
are  then  transformed  to  a  eoordinate  system  eentered  at  the  CM.  The  transformed  eorner  point 
loeations  are  stored  in  a  2-D  array.  The  first  index  in  the  array  identifies  the  objeet  type  and  the 
seeond  the  eorner  number  within  the  object.  The  first  index  becomes  the  3-D  object 
identification  number.  The  distanee  from  the  CM  to  eaeh  eorner  point  is  ealeulated  and  the 
largest  distanee  is  determined.  A  “bounding”  sphere  that  encloses  the  object  is  then  defined  with 
its  eenter  at  the  CM  and  its  radius  equal  to  the  distanee  from  the  CM  to  the  farthest  comer  point. 

Onee  the  eorner  points  have  been  defined,  the  data  defining  the  surfaee  of  the  3-D  object  type  are 
read.  The  number  of  surfaees  in  the  objeet  is  read  first.  For  eaeh  surfaee,  the  number  of  eorner 
points  in  the  surfaee  is  read.  The  surfaee  eorner  points  are  a  subset  of  the  3-D  objeet  eorner 
points.  The  seeond  index  in  the  array  of  3-D  objeet  eorner  points  is  used  to  identify  surfaee 
eorner  point  at  the  same  loeation.  These  indices  are  stored  in  a  3-D  array  that  defines  the 
surface.  The  first  index  of  the  surface  eorner  point  array  identifies  the  3-D  object,  the  second 
index  identifies  the  surfaee,  and  the  third  identifies  that  eorner  within  the  surface.  The  seeond 
index  beeomes  the  surfaee  ID  number  within  the  3-D  objeet  type. 

Once  all  the  objeet  types  have  been  defined,  the  instanees  of  the  objeet  types  in  the  environment 
are  read.  First,  the  number  of  objects  in  the  environment  is  read.  For  eaeh  3-D  object,  the 
loeation  of  its  CM  in  the  inertial  coordinate  system  is  read.  The  loeations  of  the  object’s  CM  are 
stored  in  a  one-dimensional  (1-D)  array  where  the  index  indieates  the  order  in  whieh  the 
instanees  of  the  object  in  the  environment  were  read. 

After  the  loeation  of  the  objects  CM  has  been  entered,  the  orientation  of  the  objeet  is  read.  Next, 
the  Bryant  angles  defining  the  3-D  objeet’s  orientation  in  the  inertial  eoordinate  system  are  read. 
The  Bryant  angles  (t|/,  0,  and  (|))  define  a  rotation  about  the  X  axis,  followed  by  a  rotation  about 
the  transformed  Y  axis  and  finally,  a  rotation  about  the  twiee-transformed  Z  axis.  In  other 
words,  the  object  moves  first  in  roll,  then  piteh,  and  finally  in  yaw.  The  Bryant  angles  are  stored 
in  1-D  arrays  where  the  indiees  indieate  the  order  in  whieh  the  instanees  of  the  objeet  in  the 
environment  were  read. 

Note  that  Bryant  angles  0  and  (])  are  distinct  from  the  0  and  (])  angles  used  to  defined  sean  rays  in 
spherical  coordinates.  Unfortunately,  the  standard  definition  for  both  Bryant  angles  and  spherieal 
eoordinates  use  the  same  angle  names,  resulting  in  the  overlapping  definitions  in  this  report. 

3.2  Creating  a  Range  Map  of  the  Virtual  Environment 

Onee  the  virtual  environment  has  been  defined  in  terms  of  3-D  objeets,  the  sensor  model  ean  be 
used  to  ereate  a  range  map  for  the  steering  model.  The  range  map  is  ereated  in  subroutine 
“Get  Obstaeles”.  “Get  Obstaeles”  is  ealled  by  DADS  every  time  step.  DADS  passes  the 
vehiele  position  and  orientation  at  the  eurrent  time  step  to  “Get  Obstaeles.”  The  subroutine 
passes  baek  the  range  to  the  obstaeles  surrounding  the  vehiele  along  a  set  of  pre-defmed  sean 
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rays  for  that  time  step.  The  proeess  flow  for  ereating  a  range  map  of  the  virtual  environment  is 
shown  in  figure  4. 


© 


Figure  4.  Process  flow  for  range  mapping. 
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Figure  4.  (continued) 
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Figure  4.  (continued) 

The  3-D  objects  defined  in  the  inertial  coordinate  system  are  transformed  to  the  vehicle-centered 
coordinate  system.  The  first  step  in  this  process  is  to  calculate  the  transformation  matrix  that 
rotates  coordinates  in  the  inertial  frame  into  the  vehicle  coordinate  system.  The  transformation 
matrix  required  is  calculated  from  the  set  of  three  Bryant  angles  defining  the  current  orientation 
of  the  vehicle.  Elements  of  the  three  by  three  transformation  matrix  are  calculated  by  subroutine 
“BRYANT  MATRIX”  which  is  called  by  “Get  Obstacles”. 

The  transformation  matrix,  along  with  information  about  the  3-D  objects,  is  passed  into 
subroutine  “VEH  COORDS”  which  is  called  by  “Get  Obstacles”.  “VEH  COORDS”  first 
transforms  the  CM  location  of  all  the  3-D  objects  to  vehicle-centered  coordinates.  The 
coordinate  transformation  is  performed  by  subroutine  “TRANSEORMER,”  which  is  called  by 
“VEH  COORDS”.  “TRANSEORMER”  uses  the  transformation  matrix  and  the  current  vehicle 
position  to  rotate  and  displace  the  CM  location  inertial  coordinates  into  vehicle  coordinates. 
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Once  the  objeet  CM  loeations  have  been  transformed  to  vehiele  eoordinates,  “VEH  COORDS” 
checks  the  range  to  each  object.  Each  object  is  checked  to  see  if  any  part  of  its  bounding  sphere, 
eentered  at  the  CM,  is  within  the  maximum  sensor  range  of  the  vehiele.  If  part  of  the  bounding 
sphere  falls  within  the  maximum  sensor  range,  then  a  eheek  is  made  to  see  if  any  part  of  the 
bounding  sphere  falls  within  the  angular  range  of  the  horizontal  scan  of  the  sensor.  If  some  part 
of  the  bounding  sphere  falls  within  the  horizontal  sean  range,  a  final  eheek  is  made  to  see  if  part 
also  falls  within  the  vertieal  angular  sean  range.  Those  3-D  objeets  that  have  at  least  part  of  their 
bounding  sphere  within  the  sean  pattern  of  the  sensor  are  stored  in  a  new  array.  The  array 
eontains  the  3-D  objeet  identifieation  number,  objeet  type,  range  to  the  objeet,  the  objeet  loeation 
in  vehiele  Cartesian  coordinates  and  in  vehiele  spherieal  eoordinates. 

Onee  “VEH  COORDS”  returns  to  “Get_Obstaoles,”  that  subroutine  ealls  subroutine  “SCAN” 
and  passes  the  array  of  objects  with  bounding  spheres  within  the  sensor  scan  pattern.  “Sean” 
walks  through  the  set  of  sean  rays,  starting  from  the  upper  left.  The  subroutine  first  marehes  the 
sean  rays  horizontally  left  to  right  in  steps  of  “Delta  Phi”.  When  a  horizontal  scan  is  complete, 
the  vertieal  sean  angle  is  deereased  by  “Delta  Theta”  and  the  next  eomplete  horizontal  sean  is 
initiated.  The  pattern  is  repeated  until  the  lowest  horizontal  sean  is  eompleted.  The  range  along 
all  sean  rays  is  initially  set  to  the  maximum  sensor  range.  The  range  is  deereased  only  if,  later  in 
the  proeess,  the  scan  ray  is  found  to  intercept  an  objeet  surfaee. 

Eor  eaeh  scan  ray,  a  check  is  performed  against  eaeh  object  within  the  sensor  scan  pattern  to  see 
if  the  ray  interseets  any  part  of  the  bounding  sphere.  If  the  sean  ray  does  not  intersect  the 
bounding  sphere  of  an  object,  then  the  range  along  that  ray  remains  set  to  the  maximum  sensor 
range.  If  the  sean  ray  interseets  the  bounding  sphere  of  a  3-D  objeet,  then  subroutine 
“SCAN_SURE”  is  ealled  to  see  if  it  interseets  a  surfaee  of  the  objeet. 

“SCAN  SURE”  is  used  to  determine  if  a  sean  ray  interseets  any  of  the  external  surfaees  of  a  3-D 
object.  Eirst,  the  eorner  points  of  the  objeet  to  be  examined  are  transformed  to  inertial 
eoordinates  from  object-eentered  coordinates  via  the  subroutine  “TRANSEORMER”.  The 
eorner  points  are  then  transformed  from  inertial  to  vehiele  eoordinates,  again  with 
“TRANSEORMER”.  “SCAN  SURE”  then  marches  through  the  external  surfaee  of  the  3-D 
object  in  vehicle  coordinates  to  see  if  the  scan  ray  passed  through  one  of  the  surfaces. 

The  eorner  points  of  the  surfaee  are  transformed  into  vehicle  spherieal  eoordinates.  The  eorner 
points  with  the  maximum  and  minimum  horizontal  angle,  (|),  are  found.  The  eorner  points  with 
the  maximum  and  minimum  vertieal  angle,  0,  are  also  identified.  If  the  sean  ray’s  0  and  (])  values 
do  not  fall  between  the  maximum  and  minimum  for  the  surface,  it  missed  the  surfaee.  If  it  does 
fall  with  the  maximum  and  minimum,  it  may  strike  the  surfaee.  Eor  those  rays,  we  perform  a 
further  cheek  by  calling  subroutine  “RAY  INTERSECT”. 

Subroutine  “RAY  INTERSECT”  first  ealeulates  the  unit  surfaee  normal  for  the  surfaee  in 
question.  “RAY_INTERSECT”  then  ealeulates  the  distanee  from  the  sensor  to  the  plane 
eontaining  the  surfaee  and  the  distanee  from  the  sensor  to  the  plane  along  the  sean  ray.  It  uses 
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this  information  to  calculate  the  point  where  the  sean  ray  interseets  the  plane  containing  the 
surfaee.  Onee  the  intersection  of  the  scan  ray  and  the  plane  has  been  identified,  it  must  be 
determined  if  that  point  lies  inside  the  aetual  surfaee. 

First,  the  eenter  of  the  surfaee  is  determined  and  then,  the  surfaee  is  divided  into  triangles.  Eaeh 
triangle  has  the  surfaee  center  as  one  vertex  and  two  surfaee  eorner  points  as  the  other  vertices. 
The  subroutine  then  marches  through  the  triangles  one  at  a  time,  eheeking  to  see  if  the 
interseetion  point  lies  within  it.  The  algorithm  used  to  cheek  the  triangles  was  taken  from 
Graphic  Gems  {4).  If  the  intersection  point  does  not  lie  in  any  of  the  triangles,  the  sean  ray 
missed  the  surfaee  and  the  range  remains  set  to  the  maximum  sensor  range.  If  the  intersection 
point  is  in  one  of  the  triangles,  then  the  range  is  set  to  the  distanee  from  the  sensor  to  the  plane, 
as  ealculated  before.  Subroutine  “RAY_INTERSECT”  returns  the  range  to  subroutine 
“SCANSURE”. 

In  “SCAN  SEIRE,”  eaeh  surfaee  in  the  objeet  is  checked.  If  the  sean  ray  intersects  more  than 
one  surfaee,  the  range  to  the  objeet  is  set  equal  to  the  shortest  range  to  any  of  the  surfaees  in  the 
objeet.  “SCAN_SURE”  returns  the  range  to  the  objeet  to  subroutine  “SCAN”  where  the  range 
for  that  sean  ray  is  stored  in  an  array  of  ranges  for  all  sean  rays.  The  range  array  is  2-D,  with  the 
first  index  identifying  the  vertieal  scan  angle  and  the  seeond  the  horizontal  sean  angle.  The 
range  array,  along  with  the  array  eontaining  the  horizontal  sean  angle,  (]),  and  the  vertieal  sean 
angle,  0,  form  a  range  map  of  the  environment  around  the  vehiele  in  spherieal  eoordinates.  The 
range  map  is  returned  by  “SCAN”  to  subroutine  “Get_Obstaeles”.  “Get_Obstaeles”  returns  the 
range  map  to  “Interfaee  Steering”  whieh  passes  it  onto  the  steering  model. 


4.  Obstacle  Avoidance  and  the  Previewed  Path 


The  range  map  returned  by  “Get  Obstacles”  is  3-D.  The  obstaele  avoidance  algorithm  and 
steering  algorithm  taken  from  referenee  (i)  ean  only  handle  2-D  range  maps.  The  3-D  map  is 
redueed  to  a  2-D  map  within  “Interfaee  Steering”  by  the  sorting  of  eaeh  vertieal  sliee  of  the  sean 
array  and  by  the  seleetion  of  the  minimum  range  in  that  sliee.  The  minimum  range  in  the  vertieal 
sliee  is  stored  in  the  “RD”  array.  The  horizontal  angle  “(|)”  assoeiated  with  eaeh  vertieal  sliee  is 
stored  in  the  array  “TH”.  “RD”  and  “TH”  are  passed  to  subroutine  “Avoid  Obstaeles”  along 
with  the  position  and  yaw  angle  of  the  vehicle. 

Subroutine  “Avoid  Obstaeles”  ealls  the  subroutine  “CALSRS,”  which  calculates  the  average 
range  over  the  “RD”  array  and  stores  it  in  the  variable  “RSTAR”.  The  subroutine  “CALCTH”  is 
then  ealled  and  ealeulates  the  average  of  the  produet  of  the  range  and  the  angle  over  the  arrays 
“RD”  and  “TH”.  The  average  is  stored  in  the  variable  “THSTAR”.  Einally,  “Avoid  Obstaeles” 
ealls  “CALCTH,”  whieh  ealeulates  the  range  within  the  2-D  range  map  along  the  “THSTAR” 
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angle.  If  “RSTAR”  is  greater  than  the  range  along  “THSTAR,”  then  “RSTAR”  is  redueed  by 

20%. 

“THSTAR”  and  “RSTAR”  define  a  point  on  the  preview  path  in  terms  of  vehiele-eentered  polar 
eoordinates.  The  same  point  in  the  previewed  path  in  inertial  eoordinates  is  then  ealeulated  and 
stored  in  “XSTAR”  and  “YSTAR”.  “XSTAR”  and  “YSTAR”  define  the  point  in  the  path  that 
vehiele  should  oecupy  at  the  end  of  the  preview  time. 

“Avoid  Obstaeles”  returns  “XSTAR”  and  “YSTAR”  to  ‘Tnterfaee  Steering”.  ‘Tnterfaoe_ 
Steering”  ealls  “HUMAN_STEERING ,”  which  adds  the  last  “XSTAR”  and  “YSTAR”  values  to 
the  previewed  path.  The  previewed  path  is  used  to  determine  the  future  route  of  the  vehicle.  The 
algorithm  for  “HE1MAN_STEERING”  was  covered  in  detail  in  section  2. 


5.  Interfacing  the  Human  Steering  and  Vehicle  Dynamics  Models 


The  interface  between  the  human  steering  model  and  the  vehicle  dynamics  model  is 
accomplished  with  a  series  of  EORTRAN^  subroutines.  The  external  link  into  the  DADS  vehicle 
simulation  is  via  the  user-defined  force/torque  subroutine  “ER3512”.  “ER3512”  provides  access 
to  the  UserAlgorithm  control  element  in  which  a  control  node  is  defined  to  allow  steering  control 
torques  to  be  applied  to  the  steering  actuator  model.  Movement  of  the  steering  actuator 
components,  which  is  attributable  to  the  control  torques,  causes  an  angular  change  in  the 
vehicle’s  steered  wheels  relative  to  the  vehicle  chassis,  which  in  turn  causes  the  vehicle  to 
change  direction.  Vehicle  state  information  and  vehicle  parameter  data  are  also  collected  in 
subroutine  “ER3512”  when  control  input  nodes  are  accessed.  The  control  element  input  nodes 
within  the  DADS  vehicle  model  act  as  sensors  for  collecting  vehicle  state  and  parameter  data. 
The  vehicle  data  are  passed  from  “ER3512”  to  subroutine  “VEH_STEER”.  Within 
“VEH  STEER,”  vehicle  data  undergo  unit  conversions  before  being  passed  to  subroutine 
“INTEREACE  STEERING”.  Eurther,  “VEH  STEER”  receives  steering  commands  from 
“INTEREACE  STEERING”  in  the  form  of  steering  angles  and  then  converts  these  commands  to 
steering  motor  torques.  The  commanded  steering  torque  is  then  passed  back  to  “ER35 12”. 
“INTEREACE  STEERING”  is  the  entrance  into  the  human  steering  model.  Eirst,  a  call  is  made 
to  subroutine  “HUMAN_DRIVER”  to  collect  driver  model  parameters;  next,  a  call  is  made  to 
subroutine  “GET  OBSTACLES”  to  retrieve  the  obstacle  data  information  in  the  form  of  a  range 
map.  The  range  map  data  are  used  in  the  call  to  “AVOID  OBSTACEES,”  which  generates  a 
path  for  the  vehicle  to  follow  to  avoid  the  obstacles.  “AVOID  OBSTACEES”  returns  a 
location,  XSTAR  and  YSTAR,  for  the  human  steering  model  to  steer  the  vehicle  toward. 


2 

Formula  Translator 
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XSTAR  and  YSTAR  are  passed  to  subroutine  “HUM AN_S  JEERING”  whieh  generates  and 
returns  a  eommanded  steering  angle  (DEW^). 

5.1  Steering  Commands  and  Steering  Motor  Torque  Control 

The  steering  eommand,  whieh  originates  as  a  DEW  within  subroutine  “HUMAN_STEERING,” 
is  a  torque  that  is  applied  to  the  steering  aetuator  model  within  the  DADS  vehiele  simulation. 

DEW  is  passed  from  subroutine  “HUMAN  STEERING”  via  “INTEREACE  STEERING”  to 
subroutine  “VEH  STEER”  where  it  is  eonverted  from  a  eommanded  steering  angle  to  a  steering 
motor  torque,  STRCOM.  “VEH  STEER”  employs  two  separate  algorithms  for  converting 
steering  angles  to  steering  motor  torques,  each  independently  applied,  depending  on  the  steering 
system  being  modeled.  The  first  is  a  simple  proportional  gain  function  wherein  the  output 
steering  motor  torque  is  linearly  proportional  to  the  difference  between  the  DEW  and  the  present 
steered  wheel  angle  (REWANG)  multiplied  by  the  steering  gain  coefficient  (STGAIN). 

REWANG  is  the  angle  of  the  steered  wheels  with  respect  to  the  vehicle  chassis.  REWANG  is 
typically  the  average  of  the  right  and  left  steered  wheels  when  an  Ackermann  steering  system  is 
employed.  An  Ackermann  system  uses  a  steering  linkage  geometry  in  which  the  right  and  left 
wheels  will  turn  to  different  angles,  depending  on  the  radius  of  the  arc  that  each  wheel  traverses. 

The  basic  proportional  steering  motor  torque  function  is  shown  in  equation  29. 

STRCOM=(DFW-RF WANG)  STGAIN  (29) 

The  second  or  alternate  algorithm  is  a  velocity-dependent  nonlinear  decreasing  gain  function 
wherein  the  output  steering  motor  torque  is  linearly  proportional  to  the  difference  between  the 
DEW  and  the  REWANG  multiplied  by  a  nonlinear  steering  gain  function.  The  STGAIN  is 
modified  by  a  velocity-dependent  decreasing  parabolic  function  based  on  the  vehicle  chassis 
longitudinal  velocity  and  a  velocity  gain- limiting  coefficient  (XDGEIM).  The  effect  of  this 
nonlinear  gain  function  on  the  steering  system  is  to  increase  the  applied  steering  motor  torque 
while  the  vehicle  is  at  rest  or  during  low  speed  travel.  The  increased  steering  motor  torque  is 
needed  to  offset  the  increased  resistance  to  turning  the  steered  wheels  while  at  rest  or  during  low 
vehicle  speeds.  The  velocity-dependent  steering  motor  torque  function  is  shown  in  equation  30. 

STRCOM=  (DFW-  RFWANC^  •  STGAI N  •  (^2  -  mF  ]  (30) 

in  which  XD  is  the  vehicle  chassis  longitudinal  velocity. 

The  steering  motor  torque  (STRCOM)  is  then  passed  back  to  subroutine  “ER3512”  where  it  is  linked 
to  the  output  node  for  the  UserAlgorithm  Control  element  named  ER  STEER  ACT  COMMAND. 
The  ER  STEER  ACT  COMMAND  node  within  the  DADS  simulation  control  architecture  is 
further  linked  to  the  Control  Output  One-Body  element  named  ER  STEER  ACT  TORQ. 

STRCOM  is  applied  as  a  ZE.TORQUE  to  the  ROTARY  STEER  ACT  body.  The 


3 


not  an  acronym 
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ROTARY  STEER  ACT  body  is  the  main  component  of  the  steering  motor  model  within  the 
vehicle  model.  The  steering  motor  torque  is  applied  to  the  local  z-axis  of  the  eg  triad  of  the 
ROTARY  STEER  ACT  body,  which  causes  the  steering  actuator  to  rotate  about  a  revolute  joinC 
connected  to  the  vehicle  chassis. 

The  rotary  steering  actuator  body  has  an  extended  lever  arm,  perpendicular  to  its  local  z-axis  and 
acting  as  a  simulated  pitman  arm  (i.e.,  connecting  rod),  to  convert  the  rotary  steering  motion  into 
a  translational  steering  motion.  A  DRAG  LINK  body  is  connected  via  spherical  joints  between 
the  extended  lever  of  the  rotary  steering  actuator  and  the  steering  RACK.  A  spherical  joint 
connects  two  component  bodies  in  which  relative  rotary  motion  is  allowed  about  all  axes  and 
relative  translational  motion  is  not  permitted  along  any  axis.  The  drag  link  body  transfers  the 
translational  steering  motion  into  a  lateral  movement  of  the  steering  rack  body.  The  steering 
rack  body  is  connected  via  universal  joints  to  a  left  and  a  right  tie  rod  body,  named,  respectively, 
EE  TIE  ROD  and  RE  TIE  ROD.  A  universal  joint  connects  two  component  bodies  in  which 
relative  rotational  motion  is  allowed  about  two  axes  and  relative  translational  motion  is  not 
allowed  along  any  axis.  The  individual  left  and  right  tie  rod  bodies  transfer  the  lateral  steering 
rack  motion  to  their  respective  wheel  hub  bodies,  EE  WHEEE  HUB  and  RE  WHEEE  HUB. 
Each  wheel  hub  body  has  a  lever  arm  extended  perpendicular  to  its  steering  axis;  this  arm  acts  as 
a  simulated  steering  knuckle.  The  tie  rod  bodies  are  connected  via  spherical  joints  to  their 
respective  steering  knuckles.  These  connections  allow  the  lateral  steering  motion  to  be 
converted  into  angular  motion  about  the  left  and  right  steering  axes. 

Wheel  bodies,  EE  WHEEE  and  RF  WHEEE,  are  connected  to  their  respective  wheel  hub  bodies 
by  revolute  joints.  These  joint  connections  allow  the  wheel  bodies  to  rotate  relative  to  the 
vehicle  chassis.  The  wheel  bodies  form  the  basis  for  the  individual  tire  models  that  generate  the 
forces  for  displacing  the  vehicle  chassis.  Eateral  displacement  of  the  vehicle  chassis  is 
accomplished  by  the  generation  of  lateral  forces  within  the  tire  models.  The  lateral  forces  for 
turning,  known  as  cornering  forces,  are  created  when  the  rolling  wheel  bodies  have  an  angular 
displacement  about  their  steering  axes,  relative  to  the  vehicle  chassis.  This  angular  displacement 
of  the  wheel  body  creates  a  slip  angle  within  the  rolling  tire  model.  Slip  angle  is  defined  as  the 
difference  between  the  steered  angle  of  the  tire  and  the  actual  angle  of  tire  travel  relative  to  the 
vehicle  chassis.  Slip  angle  is  attributable  to  the  deformation  or  twisting  of  the  tire  carcass  during 
cornering,  which  produces  a  resultant  lateral  force.  This  lateral  force  is  transmitted  from  the  tire 
model  through  the  wheel  body  to  the  vehicle  chassis,  which  results  in  the  lateral  displacement  or 
turning  of  the  vehicle. 


revolute  joint  is  a  connection  between  two  component  bodies  in  which  rotary  motion  is  permitted  about  a  single  axis 
(typically  the  z-axis),  and  relative  translational  motion  is  not  permitted  along  any  axis. 
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5.2  Vehicle  Speed  and  Drive  Torque  Control 

The  DADS  vehicle  velocity  during  simulation  runs  is  controlled  through  a  UserAlgorithm 
Control  element  that  provides  a  control  node  to  the  user-defined  force/torque  subroutine 
“FR3512”.  The  UserAlgorithm  Control  element,  SPEED  CONTROL,  has  an  output  node 
known  as  DRIVE  TORQUE.  DRIVE  TORQUE  is  accessed  through  subroutine  “ER3512,” 
which  calls  the  velocity-controlling  subroutine  “SPEED”.  The  control  node  information 
RR_WHEEE_RATE,  which  provides  the  angular  rate  of  the  right  rear  wheel,  is  passed  to 
subroutine  speed.  The  algorithm  within  subroutine  SPEED  is  a  simple  velocity  controller  in 
which  driving  and  braking  torques  are  generated,  based  on  the  angular  rate  of  the  simulated 
vehicle’s  right  rear  wheel.  A  relatively  constant  vehicle  velocity  is  attained  by  the  generation  of 
a  drive  torque  output  that  is  proportional  to  the  angular  rate  error  of  the  right  rear  wheel.  Eirst,  a 
desired  angular  wheel  rate  is  set;  then,  the  desired  wheel  rate  is  subtracted  from  the  actual  rate  of 
the  right  rear  wheel.  The  differencing  creates  an  angular  rate  error,  which  is  then  multiplied  by  a 
torque  gain  constant.  The  resulting  torque  output  is  then  limited  to  avoid  generating  excessive 
braking  or  driving  torques.  The  output  drive  torque  is  then  passed  back  to  the  output  control 
node,  DRIVE  TORQUE,  via  subroutine  “ER3512”.  The  control  node,  DRIVE  TORQUE,  is 
linked  to  each  of  four  Control  Output  One -Body  elements.  The  Control  Output  One-Body 
elements,  RR  WHEEE  TORQUE,  LR  WHEEE  TORQUE,  RE  WHEEE  TORQUE,  and 
EE  WHEEE  TORQUE,  represent  drive  and  braking  torque  being  distributed  to  each  of  the 
vehicle’s  four  wheels. 

5.3  Vehicle  State  and  Data  Parameters 

The  vehicle  state  and  data  parameters  are  collected  within  the  DADS  vehicle  simulation  and  then 
communicated  to  the  human  driver  model  through  subroutine  “ER3512”.  The  vehicle  state  and 
parameter  information  is  sensed  and  collected  from  the  vehicle  dynamics  model  in  the  form  of 
control  node  information  and  rigid  body  array  data.  The  DADS  global  coordinates  are  equiva¬ 
lent  to  the  inertial  coordinates  in  the  human  steering  model.  The  control  node  information  is 

CHASSIS  X  POS  -  the  global  longitudinal  position  of  the  vehicle  chassis 

CHASSIS  Y  POS  -  the  global  lateral  position  of  the  vehicle  chassis 

CHASSIS  Z  POS  -  the  global  vertical  position  of  the  vehicle  chassis 

CHASSIS  X  VEE  -  the  global  longitudinal  velocity  of  the  vehicle  chassis 

CHASSIS  Y  VEE  -  the  global  lateral  velocity  of  the  vehicle  chassis 

CHASSIS  Z  VEE  -  the  global  vertical  velocity  of  the  vehicle  chassis 

CHASSIS  ROEE  ANG  -  the  roll  angle  of  the  vehicle  chassis 

CHASSIS  PITCH  ANG  -  the  pitch  angle  of  the  vehicle  chassis 

CHASSIS_YAW_ANG  -  the  yaw  angle  of  the  vehicle  chassis 

CHASSIS  ROEE  RATE  -  the  roll  rate  of  the  vehicle  chassis 

CHASSIS  PITCH  RATE  -  the  pitch  rate  of  the  vehicle  chassis 

CHASSIS  YAW  RATE  -  the  yaw  rate  of  the  vehicle  chassis 

CHASSIS  EOCAL  Y  VEE  -  the  local  lateral  velocity  of  the  vehicle  chassis 

ER  STEER  ACT  SENSOR  -  the  angle  of  the  rotary  steering  actuator  with  respect  to  the 
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vehicle  chassis 

FR_WHEEL_WRT_CHASSIS  -  the  angle  of  the  front  wheel  with  respect  to  the  vehicle  chassis 
RE  WHEEE  RATE  -  the  angular  rate  of  the  right  front  wheel 
RR  WHEEE  RATE  -  the  angular  rate  of  the  right  rear  wheel 

Rigid  body  array  data  are 

NUMWHE  -  number  of  wheels  on  the  vehicle 
NUMRB  -  number  of  rigid  bodies  within  the  vehicle  simulation 
CRSTER(l)  -  cornering  stiffness  of  the  right  front  wheel 
CRSTER(2)  -  cornering  stiffness  of  the  right  rear  wheel 
CRSTEE(l)  -  cornering  stiffness  of  the  left  front  wheel 
CRSTEE(2)  -  cornering  stiffness  of  the  left  rear  wheel 
VMASS  -  mass  of  the  vehicle 
IZZ5  -  yaw  moment  of  inertia  of  the  vehicle 
ZER(l)  -  load  on  the  right  front  tire 
ZER(2)  -  load  on  the  right  rear  tire 
ZEE(l)  -  load  on  the  left  front  tire 
ZEE(2)  -  load  on  the  left  rear  tire 

Control  node  information  is  collected  within  the  vehicle  simulation  through  the  use  of  Control 
Output  One-Body  elements  and  Control  Output  Two-Body  elements.  We  access  the  Control 
Output  One-Body  element,  CHASSIS  PITCH  ANG,  for  example,  by  specifying  the  simulation 
body  of  interest,  CHASSIS  SENSOR,  the  body’s  triad  where  the  sensing  will  take  place, 

CHASSIS  SENSOR  CG,  and  the  type  of  data  to  collect  from  this  location.  Eor  this  example,  to 
collect  the  chassis  pitch  angle,  the  information  collected  was  the  second  Bryant  angle.  A  triad  is 
defined  within  the  simulation  architecture  as  a  3-D  coordinate  origin.  We  access  the  Control 
Output  Two-Body  element,  ER  STEER  ACT  SENSOR,  for  example,  by  specifying  the  first 
simulation  body  of  interest,  CHASSIS,  the  first  body’s  triad,  ROT_STR_ACT_MNT,  the  second 
simulation  body  of  interest,  ROTARY  STEER  ACT,  the  second  body’s  triad, 

ROTARY  STEER  ACT,  and  the  type  of  data  to  be  collected.  The  information  collected  in  this 
example  is  the  angular  difference  about  the  z-axes  of  the  triads  of  the  two  bodies.  Control  node 
information,  such  as  the  two  examples  given,  is  then  passed  from  the  DADS  simulation  by  a 
GETNODE  function  call  within  the  user-defined  subroutine  “ER3512”.  This  information  is  then 
passed  to  subroutine  “VEH_STEER”  for  use  within  the  human  driver  model. 

Rigid  body  array  data  are  collected  directly  through  subroutine  “ER3512”  by  a  call  to  the 
function  INDXAR.  This  function  returns  the  index  of  the  desired  data  element  from  the  array  of 
interest.  This  index  is  then  used  to  return  the  desired  value  from  the  array.  The  arrays  that  may 
be  searched  via  the  INDXAR  function  are 

A  -  all  real  data  for  the  system 
lA  -  all  integer  data  for  the  system 
Q  -  global  generalized  coordinates 
QD  -  global  generalized  velocities 
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QDD  -  global  generalized  accelerations 
FRC  -  global  generalized  forces 


To  find  the  index  of  the  desired  element,  the  function  call  to  INDXAR  must  include  the  name  of 
the  array  being  searched,  the  dimension  of  the  solution  (2-D  or  3-D),  the  module  number  of  the 
element,  the  instance  of  the  element,  the  index  of  the  element  array,  and  the  “lA”  array.  For 
example,  for  us  to  acquire  the  value  for  ZFR(l),  the  normal  force  on  the  right  front  tire,  the 
following  must  be  specified  in  the  call  to  INDXAR: 


in  which 


ZFR(l)  =  A(INDXAR(‘A’,3,31,3,42,IA)) 


A  -  the  name  of  the  real  data  array 
3  -  indicates  a  3-D  solution 
3 1  -  tire  force  element  module  number 

3  -  the  instance  of  the  tire  element  (the  right  front  tire  is  the  third  instantiation  of  the  tire 
element) 

42  -  the  index  of  the  normal  force  in  the  tire  element 
lA  -  the  name  of  the  integer  data  array 

This  value,  along  with  the  other  values  acquired  from  the  rigid  body  array,  are  then  passed  from 
subroutine  “FR3512”  to  subroutine  “VEH  STEER”.  The  rigid  body  array  data  and  the  control 
node  information  are  then  passed  to  subroutine  ‘TNTEREACE_STEERING”  for  use  within  the 
human  driver  model. 


6.  Vehicle  Dynamics  Model 


6,1  High  Mobility  Multipurpose  Wheeled  Vehicle  (HMMWV)  Model 

The  DADS  vehicle  dynamics  model  chosen  for  interfacing  to  the  human  driver  model  was  a 
model  of  the  HMMWV  that  was  developed  by  the  Elniversity  of  Iowa’s  Center  for  Virtual 
Proving  Ground  Simulation.  A  HMMWV  model  was  selected  since  it  would  likely  enable  a 
more  direct  comparison  with  the  original  driver  model  DADS  interfacing  results.  The  DADS 
HMMWV,  developed  by  the  University  of  Iowa,  is  an  advanced  22-body  model  that  includes  the 
ability  to  simulate  propulsion,  braking,  and  steering.  During  development  of  the  interface 
between  the  human  driver  model  and  the  DADS  vehicle  dynamics  model,  a  surrogate  DADS 
vehicle  model  was  employed.  This  vehicle  model  contained  a  rotary,  hydraulic  steering  system, 
and  the  development  of  the  steering  control  algorithms  focused  upon  this  type  of  steering  system. 
When  the  human  driver  model  was  interfaced  to  the  University  of  Iowa-developed  HMMWV,  a 
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few  modifications  of  the  model  were  required  to  enable  the  use  of  the  rotary  steering  controller. 
Further,  the  HMMWV  model  required  the  addition  of  a  number  of  control  architecture  elements. 

6.2  Steering  Control  Modifications 

The  HMMWV,  as  obtained  from  University  of  Iowa,  employs  a  “rack  and  pinion”  type  steering 
system,  in  which  the  steering  rack  is  driven  laterally  by  a  translational  actuator.  The  ends  of  the 
steering  rack  are  attached  to  tie  rods  at  a  joint,  which  transmit  the  lateral  translational  motion  to 
the  steering  knuckles.  The  steering  knuckles,  which  mount  the  wheel  hubs  and  wheels,  convert 
the  translational  motion  to  angular  motion  for  steering  the  wheels.  The  human  driver  model 
employs  a  rotary  steering  actuator  control  algorithm;  thus,  a  rotary  steering  actuator  model  needed 
to  be  added  to  the  HMMWV  model.  The  modification  required  disabling  the  HMMWV  model’s 
present  translational  actuator  and  the  fitting  of  a  rotary  actuator  and  required  linkage.  A  rotary 
steering  actuator  body,  ROTARY_STEER_ACT,  was  connected  to  the  HMMWV  chassis  by  a 
revolute  joint.  The  rotary  steering  actuator  body  has  an  extended  lever,  acting  as  a  pitman  arm.  A 
drag  link  body,  DRAG  LINK,  is  connected  to  the  pitman  arm  via  a  spherical  joint.  The  drag  link 
body  uses  another  spherical  joint  at  its  other  end  to  connect  to  the  steering  rack  body,  RACK. 

This  actuator  and  linkage  model  convert  the  rotary  steering  input  into  the  lateral  translational 
motion  required  by  the  HMMWV  model.  The  rotary  steering  input,  STRCOM,  is  the  commanded 
steering  motor  torque  from  the  vehicle  steering  interface  model,  “VEH_STEER”.  To  properly 
control  the  steering  torque,  a  DADS  friction  element  was  developed  and  included  in  the  steering 
motor  model.  The  friction  element  acts  as  a  damper  in  the  feedback  steering  control  system.  The 
friction  element,  ROT  STEER  ACT  ERICT,  takes  the  form  of  bearing  friction  within  the  rotary 
steering  actuator  body  and  is  applied  to  the  revolute  joint,  REV  ROTSTR  ACT  CHASS,  that 
connects  the  rotary  steering  actuator  to  the  vehicle  chassis.  The  bearing  friction  is  based  on  the 
following  specifications: 

bearing  radius  -  2.0  inches 

bearing  height  -  4.0  inches 

static  coefficient  of  friction  -  0.2 

dynamic  coefficient  of  friction  -  0.15 

linear  velocity  threshold  -  0.001  inch/second 

The  frictional  damper  is  necessary  within  the  steering  motor  model  to  alleviate  steering  system 
control  jitter  during  transitional  steering  commands. 

6.3  Control  Elements  for  Vehicle  State  Sensing  and  Commanded  Actuation 

Control  element  additions  are  required  in  the  DADS  vehicle  model  for  the  human  driver  to 
receive  vehicle  state  information  and  to  output  driving  commands.  A  series  of  Control  Input 
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One-Body  and  Control  Input  Two-Body  elements  was  added  to  the  University  of  Iowa 
HMMWV  model.  A  eomprehensive  listing  of  the  eontrol  input  elements  is  ineluded  in 
seetion  4.3.  These  eontrol  input  elements  funetion  as  virtual  sensors  to  aequire  the  present 
vehiele  state  during  a  simulation,  whieh  is  required  by  the  human  driver  steering  eontroller. 
Control  Output  One-Body  elements  were  added  to  the  vehiele  model  to  funetion  as  virtual 
aetuators  eontrolled  by  the  human  driver  model.  The  eontrol  output  elements  are 

FR  STEER  ACT  TORQ  -  for  aetuating  the  rotary  steering  aetuator 

RR  WHEEE  TORQUE  -  for  powering  the  right  rear  wheel 

ER  WHEEE  TORQUE  -  for  powering  the  left  rear  wheel 

RE  WHEEE  TORQUE  -  for  powering  the  right  front  wheel 

EE  WHEEE  TORQUE  -  for  powering  the  left  front  wheel 

These  eontrol  output  elements  allow  the  human  driver  model  to  apply  eontrol  torques  to  the 
vehiele  dynamies  model  during  a  simulation  run  to  alter  its  position  and  veloeity. 


7.  Validation  of  HMMWV  Results 


Eigure  5  shows  a  view  of  the  slalom  eourse  and  the  HMMWV  model.  The  slalom  eourse 
eonsists  of  a  single  lane  entry,  a  two-lane  seetion  with  two  square  obstaeles,  and  a  single  lane 
exit.  The  boundaries  of  the  eourse  are  lined  with  “Jersey”  barriers.  The  first  obstaele  abuts  the 
left-hand  Jersey  barrier  boundary.  The  seeond  obstaele  abuts  the  right-hand  Jersey  barrier 
boundary.  Eigure  6  shows  a  series  of  frames  of  the  HMMWV  model  maneuvering  through  the 
slalom  eourse. 

The  slalom  eourse  has  the  same  geometry  in  the  X-Y  plane  as  the  eourse  deseribed  in  referenee 
(1).  The  entry  and  exit  lanes  are  12  feet  wide  and  175  feet  long.  The  two-lane  seetion  is  24  feet 
wide  and  324  feet  long.  The  two  square  obstaeles  measure  12  feet  on  eaeh  side.  The  first 
obstaele  is  100  feet  from  the  start  of  the  two-lane  seetion.  The  seeond  obstaele  is  212  feet  from 
the  start  of  the  two-lane  seetion. 

Eigure  7  shows  the  results  of  a  test  run  of  the  HMMWV  model  through  the  slalom  eourse.  The 
neuromuseular  delay  and  “look-ahead”  time  used  are  the  same  as  those  in  referenee  (1).  The 
HMMWV  aeeelerates  in  the  entry  lane  and  reaehes  40  mph  as  the  vehiele  enters  the  two-lane 
seetion.  The  vehiele  then  maintains  40  mph  for  the  rest  of  the  eourse.  This  is  the  speed  used  for 
similar  runs  in  referenee  (7). 


25 


Figure  6.  FiMMWV  maneuvering  through  the  slalom  course. 


26 


Figure  7.  Planned  and  actual  vehicle  path  through  slalom  course. 

In  Figure  7,  the  heavy  dark  blue  lines  are  the  Jersey  barriers  defining  the  course  boundaries.  The 
light  blue  line  is  the  planned  or  previewed  path.  Each  of  the  diamond  symbols  indicates  an 
“XSTAR,YSTAR”  pair  generated  by  subroutine  “Avoid  Obstacles”.  The  dark  purple  line  is  the 
path  followed  by  the  vehicle’s  center  of  mass.  Each  of  the  squares  is  the  vehicle’s  position  as 
reported  by  the  DADS  simulation  model. 

Eigure  8  shows  a  comparison  between  the  steering  model  developed  in  this  report,  the  existing 
steering  model,  and  experimental  data  in  reference  (7).  The  slalom  course,  vehicle  type,  and 
speed  driven  are  the  same  in  all  three  cases.  The  red  line  in  the  figure  is  the  path  of  the  vehicle’s 
eg  calculated  by  the  model  described  in  this  report.  The  heavy  black  line  is  the  model  runs  from 
reference  (1).  The  thin  black  lines  are  experimental  runs  performed  by  human  drivers  as 
reported  in  reference  (7).  The  red  path  lies  close  to  or  within  the  set  of  experimental  paths,  thus 
validating  that  the  current  model  does  a  reasonable  job  of  simulating  human  steering  for  this 
vehicle  on  this  course. 
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Figure  8.  Comparison  of  steering  models  and  experimental  results. 


8.  Conclusions 


The  steering  model  described  in  this  report  successfully  simulates  a  human  driving  a  vehicle 
through  a  series  of  3-D  obstacles.  The  model  simulates  the  solid  geometry  of  a  range  finding, 
driving  sensor  attached  to  the  vehicle  and  operating  in  a  simple  3-D  virtual  environment.  The 
model  also  simulates  a  control  loop  representing  a  human  steering,  with  neuromuscular  delay 
and  look-ahead  time.  The  sensor  model  and  the  steering  model  are  coupled  to  a  detailed  vehicle 
model  generated  in  the  commercial  off-the-shelf  (COTS)  simulation  environment  DADS. 

If  a  human  in  the  loop  is  used  to  provide  steering  commands  to  the  vehicle  dynamics  model,  then 
the  model  must  be  simplified  to  run  in  real  time.  If  a  predetermined  series  of  steering  commands 
is  used,  these  commands  will  not  reflect  the  feedback  that  occurs  between  the  steering 
commands  and  vehicle  dynamics.  This  model  allows  for  complex  vehicle  tests  to  be  simulated 
while  the  feedback  is  maintained  between  the  steering  commands  and  the  vehicle  dynamics  but 
without  a  human  in  the  loop.  In  this  way,  complex  tests  can  be  simulated  efficiently  on  small 
computers  at  slower  than  real  time.  Running  the  simulation  in  the  background  can  provide  input 
to  parametric  studies.  These  studies  estimate  which  candidate  vehicle  designs  will  perform  best 
on  a  standard  test  course.  The  results  could  be  used  to  guide  vehicle  design  or  for  better 
planning  of  physical  experiments. 
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Appendix  A.  Code  Makefile 


•k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 

*  CODE  MAKEFILE  * 

•k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 


#cc  =  cc 

ttCFLAGS  =  -g  -strictlEEE  -static 

#NOTE :  Use  SGI  make  command  /usr/bin/make 

## - IRIX  defs - 

CXX  =  CC 

CFLAGS  =-g  -O  -D_POSIX_PTHREAD_SEMANTICS  \ 

-I/other/OTB . rp/ include/global  \ 

-I /other /0TB . rp/ include/ libinc/  \ 

-I/other/OTB . rp/ include/gnuinc  \ 

-I/other/OTB . rp/ libsrc/ libctdb/  \ 

-I/other/OTB . rp/ libsrc/ libctdb/ compiler/  \ 

-I/other/OTB . rp/ 

LDFLAGS2=  -g  -Im  -L/other/OTB . rp/ lib 

LIBS2=  -Ictdb  -Icoordinates  -Igeometry  -Igcs  -Iconstants  -Ivecmat  -Ireader  - 
lgeo3d  -Ibasic  -Im 

FTNLIBS=-L  /usr/lib/DCC  -Ic  -Iftn 
#CCLIBS=  -L  /usr/lib/CC  -I  /usr/include 

.SUFFIXES:  .o  .f  .bd 

DADSLIBDIR  =  /e3a/dads95/dadslib/ 

MEXLIBDIR  =  /e3a/dads95/dadslib/sgi64/ 

LMGRLIB  =  liblmgr.a 

DADSLIBSMEX=  \ 

${MEXLIBDIR}blockda.o  \ 

$ {MEXLIBDIR} revbd.o  \ 

${MEXLIBDIR}xdummy.o  \ 

${MEXLIBDIR}mod3d.a  \ 

$ {MEXLIBDIR} analysis. a  \ 

$ {MEXLIBDIR} super3d. a  \ 

${MEXLIBDIR}mod3d.a  \ 

$ {MEXLIBDIR} analysis. a  \ 

${MEXLIBDIR}mod3d.a  \ 

${MEXLIBDIR}controls3d.a  \ 

$ {MEXLIBDIR} controls. a  \ 

${MEXLIBDIR}harwell.a  \ 

$ {MEXLIBDIR} tools. a  \ 

$ {MEXLIBDIR} daf tools. a  \ 

${MEXLIBDIR}mathtools.a  \ 

$ {MEXLIBDIR} dadsblas. a  \ 

$ {MEXLIBDIR} $ {LMGRLIB}  \ 
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${MEXLIBDIR}libparent.a  \ 
${MEXLIBDIR}libcp.a  \ 
${MEXLIBDIR}ctools.a  \ 

$ {MEXLIBDIR} expressionparser . a  \ 
$ {MEXLIBDIR} mxx dummy . o  \ 

$ {MEXLIBDIR} tecdummy.o  \ 

$ {MEXLIBDIR} cgdummy.o  \ 

$ {MEXLIBDIR} ezSdummy.o 

DADSLIBS=  \ 

${DADSLIBDIR}blockda.o  \ 
${DADSLIBDIR}revbd.o  \ 
${DADSLIBDIR}xdummy.o  \ 
${DADSLIBDIR}mod3d.a  \ 
${DADSLIBDIR} analysis. a  \ 
${DADSLIBDIR}super3d.a  \ 
${DADSLIBDIR}mod3d.a  \ 
${DADSLIBDIR} analysis. a  \ 
${DADSLIBDIR}mod3d.a  \ 
${DADSLIBDIR}controls3d.a  \ 
${DADSLIBDIR} controls. a  \ 
${DADSLIBDIR}harwell.a  \ 
${DADSLIBDIR}tools.a  \ 
${DADSLIBDIR}daftools.a  \ 
${DADSLIBDIR}mathtools.a  \ 
${DADSLIBDIR}dadsblas.a  \ 
${DADSLIBDIR}${LMGRLIB}  \ 
${DADSLIBDIR}libparent.a  \ 
${DADSLIBDIR}libcp.a  \ 
${DADSLIBDIR}ctools.a  \ 

$ {DADSLIBDIR} expressionparser . a  \ 

$ {DADSLIBDIR}mxxdummy . o  \ 

$ {DADSLIBDIR} tecdummy.o  \ 

$ {DADSLIBDIR} cgdummy. o  \ 

$ {DADSLIBDIR} ezSdummy. o 

FORT_OBJS  =  \ 

Get_0bstacles2 . o\ 

Human_Driver . o\ 

Avoid_Obstacles . o\ 

Human_Steering . o\ 
calcrs . o\ 
calcth . o\ 
checkrth . o\ 
gmprd . o \ 
main . o\ 

Interf ace_Steering . o\ 
fr3512 .o\ 
veh_steer . o\ 
speed. o\ 
tranxy . o 

ndads3d  :  ${FORT_OBJS} 

f77  -static  -g  -n32  -mips4  \ 

-o  ndads3d  \ 

$ { FORT_OB JS }  \ 

${FTNLIBS}  \ 

${CFLAGS}  \ 
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${LDFLAGS2}  \ 

$(LIBS2)  \ 

${DADSLIBDIR}progrin.o  \ 

$  {DADSLIBDIR}inatduininy .  o  \ 

$(DADSLIBS)  \ 

-1C  -Ic  -V 

mexfile:  ${FORT_OBJS} 
mex  -V  \ 

-output  dads3d.mexsg64  $ (DADSLIBDIR) dads3d. c  \ 

${FORT_OBJS}  \ 

$ (DADSLIBDIR) cxxhead.o  \ 

$ (DADSLIBSMEX)  -1C  -Ic 

main . o :  main . f 

@echo 

@echo  Compiling 

f77  $(CFLAGS)  -static  -c  -g  -n32  -mips4  main.f  -o  main.o 

@echo  -  Done  - 

@echo 

Avoid_Obstacles . o :  Avoid_Obstacles . f 

@echo 

@echo  Compiling 

f77  $(CFLAGS)  -Static  -c  -g  -n32  -mips4  Avoid_Obstacles . f  -o 
Avoid_Obstacles . o 

@echo  -  Done  - 

@echo 


calcrs . o : 

calcrs . f 

@echo 

@echo 

Compiling 

f77  $ 

(CFLAGS)  -static 

-c 

-g  -n32 

-mips4 

calcrs . f  -o  calcrs. o 

@echo 

@echo 

-  Done  - 

calcth . o : 

calcth . f 

@echo 

@echo 

Compiling 

f77  $ 

(CFLAGS)  -static 

-c 

-g  -n32 

-mips4 

calcth. f  -o  calcth. o 

@echo 

@echo 

-  Done  - 

checkrth . o : 

checkrth . f 

@echo 

@echo 

Compiling 

f77  $ 

(CFLAGS)  -static 

-c 

-g  -n32 

-mips4 

checkrth. f  -o  checkrth 

@echo 

@echo 

-  Done  - 

Get_0bstacles2 . o :  Get_0bstacles2 . f 

@echo 

@echo 

Compiling 

til  $(CFLAGS)  -static 

-c 

-g  -n32 

-mips4 

Get_0bstacles2 . f  -o 

Get_0bstacles2 . o 

@echo 

@echo 

-  Done  - 
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gmprd . o :  gmprd . f 

@echo 

@echo  Compiling 

f77  $(CFLAGS)  -static  -c  -g  -n32  -mips4  gmprd. f  -o  gmprd. o 

@echo  -  Done  - 

@echo 

Human_Driver . o :  Human_Driver . f 

@echo 

@echo  Compiling  $*.f... 

f77  $(CFLAGS)  -static  -c  -g  -n32  -mips4  Human_Driver . f  -o 
Human_Driver . o 

@echo  -  Done  - 

@echo 

Human_Steering . o :  Human_Steering . f 

@echo 

@echo  Compiling  $*.f... 

f77  $(CFLAGS)  -static  -c  -g  -n32  -mips4  Human_Steering . f  -o 
Human_Steering . o 

@echo  -  Done  - 

@echo 

Interf ace_Steering . o :  Interf ace_Steering . f 

@echo 

@echo  Compiling  $*.f... 

f77  $(CFLAGS)  -static  -c  -g  -n32  -mips4  Interf ace_Steering . f  -o 
Interf ace_Steering . o 

@echo  -  Done  - 

@echo 

fr3512.o:  fr3512.f 

@echo 

@echo  Compiling  $*.f... 

f77  $(CFLAGS)  -static  -c  -g  -n32  -mips4  fr3512.f  -o  fr3512.o 

@echo  -  Done  - 

@echo 

veh_steer.o:  veh_steer.f 

@echo 

@echo  Compiling  $*.f... 

f77  $(CFLAGS)  -static  -c  -g  -n32  -mips4  veh_steer.f  -o  veh_steer.o 

@echo  -  Done  - 

@echo 

speed . o :  speed . f 

@echo 

@echo  Compiling  $*.f... 

f77  $(CFLAGS)  -Static  -c  -g  -n32  -mips4  speed. f  -o  speed. o 

@echo  -  Done  - 

@echo 

tranxy . o :  tranxy . f 

@echo 

@echo  Compiling  $*.f... 

f77  $(CFLAGS)  -Static  -c  -g  -n32  -mips4  tranxy. f  -o  tranxy. o 

@echo  -  Done  - 

@echo 
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•k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 

*  FORTRAN  CODE  FOR  DRIVER  MODEL  * 

•k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 


SUBROUTINE  Avoid_Obstacles  (THETRAD,  DELTHRAD,  NPOWER,  XV,  YV, 
1  ZV,  PSIRAD,  UMAX,  RD,  TH, 

1  XSTAR,  YSTAR) 


double  precision  RD (200 ), TH (200 ), THETRAD,  DELTHRAD, 
&  XV, YV, ZV, PSIRAD, XSTAR, YSTAR, 

&  PI , SUMl , SUM2 , TRSTAR, TTHETA, RSTAR, 

&  THSTAR 

DATA  PI  /3 . 1416/ 


SUMl  =0.0 
SUM2  =0.0 
DO  1000  K  =  1,  UMAX 
L  =  (UMAX  -  K)  +  1 
SUMl  =  RD(L)*TH(L)  +  SUMl 
SUM2  =  RD(L)  +  SUM2 


CONTINUE 

TRSTAR  =  SUM2/JMAX 
TTHETA  =  SUM1/SUM2 


CALL  CALCRS(JMAX, 

RD, 

TH, 

THETRAD , 

DELTHRAD , 

RSTAR) 

CALL  CALCTH(JMAX, 

RD, 

TH, 

THETRAD , 

DELTHRAD , 

NPOWER,  THSTAR) 

CALL  CHECKRTH ( UMAX , RD , TH , RSTAR , THSTAR ) 

XSTAR  =  RSTAR*COS (PSIRAD  +  THSTAR  )  +  XV 
YSTAR  =  RSTAR*SIN( PSIRAD  +  THSTAR  )  +  YV 


return 

END 

C 

C 

C  Subroutine  Get_Obstacles  reads  information  on  3D  obstacles  in  obstacle 
C  centered  coordinates,  transfroms  to  inertial  cartesian  coordinates  and 
C  the  to  vehicle  center  coordinate  both  cartesian  and  polar.  The 
subroutine 

C  then  calculates  the  distance  to  the  obstacles  along  a  set  of  sensor  rays. 

C 

C 
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c 


(FIRST,  TDelta,  SMAX_PHI ,  SMAX_THETA, 
RMAX , SDELTA_PHI ,  SDELTA_THETA, 

XVE,  YVE,  ZVE,  Psi, Theta,  Phi, 
SCAN_RANGE , SCAN_PHI , SCAN_THETA) 

dimension  NPOLY (10) 


SUBROUTINE  Get_Obstacles 

1 

1 

1 


Surfaces  in  object  types.  #  surfaces,  #  corners  in  surface,  corner  indexes 
DIMENSION  NUM_SURF ( 10 ) ,  NUM_CORN (10,10) ,  INDEX_CORN (10,10,8) 

dimension  IOBJ_TYPE (200 ) , ID (200 ) 


& 

& 

& 

& 

& 

& 

& 

& 

& 

& 

& 

& 

& 


double  precision  TDelta , SMAX_PHI , SMAX_THETA, RMAX, SDELTA_PHI , 
SDELTA_THETA , XVE , YVE , ZVE , Ps i , Theta , Phi , 
SCAN_RANGE (40,40) , SCAN_THETA (40), 

SCAN_PHI (40)  ,RD(200)  ,TH(200)  ,XTD(18)  , 
YTD(18)  , ZTD(18)  ,XTC (10)  , YTC (10)  , ZTC (10)  , 

XT (10, 18) ,YT(10,18) ,ZT(10,18) , 

XCI (200) , YCI (200) , 

ZCI (200) , R_Bound ( 2 0 0 ) , 

E_PSI (200) , E_THETA ( 2 0 0 ) , E_PHI (200) , 

XCV (200), YCV (200), ZCV (200), RV (200), 

VPHI (200) , VTHETA (200) , DELTA_ANGLE (200) , 
A(3,3) ,B(3,3) ,XTSUM, YTSUM, ZTSUM,R, PSI_E, 
THETA_E , PHI_E , CXE , CYE , CZE , CX , CY , CZ , XV, YV, 
ZV, PI, EPSILON 


integer  num_theta , kmax , count 


LOGICAL  FIRST 
CHARACTER* 50  COMMENT 


DATA  PI  /3 . 1415926545/ 

DATA  EPSILON  / 0 . 000000001/ 
data  count  /!/ 


C 

C 

C 

C 


IF (FIRST)  THEN 

Read  in  obstacles  from  files 

OPEN (UNIT=9 , FILE= ' Course_Obstacles . txt ' ) 
REWIND  9 


C 


OPEN (UNIT=4 , FILE= ' Obstacles_Check . txt ' ) 
REWIND  4 


C 


READ  (  9 , * )  COMMENT 


oo  oo  oo  o  ooo  ooooooo  oo 


c 


READ  ( 9 , * ) ,  COMMENT 

READ  (  9  ,  * )  NUM_TYPE 

DO  10  I  =  1,  NUM_TYPE 


Initialize  the  sum  of  the  corner  points 
XTSUM  =  0 . ODO 
YTSUM  =  0 . ODO 
ZTSUM  =  0 . ODO 

READ  ( 9 , * )  COMMENT 
READ  ( 9 , * )  COMMENT 

READ  (  9 , * )  NPOLY ( I ) 

READ  ( 9 , * )  COMMENT 
READ  ( 9 , * )  COMMENT 
DO  20  J  =  1,  NPOLY (I) 

READ  (9,*),  XTD(J) , YTD(J) , ZTD(J) 

XTD(J)  =  XTD(J) /0.3048 
YTD(J)  =  YTD(J) /0.3048 
ZTD(J)  =  ZTD(J) /0.3048 

PRINT  15,  I, J,XTD(J) , YTD(J) , ZTD(J) 

15  FORMAT ("TYPE  =  ",I5,"  Corner  #  =  ",I5,"  XTD  =  ", 

1  F10.7,"  YTD  =  ",F10.7,"  ZTD  =  ",F10.7) 

XTSUM  =  XTSUM  +  XTD(J) 

YTSUM  =  YTSUM  +  YTD(J) 

ZTSUM  =  ZTSUM  +  ZTD(J) 

20  CONTINUE 

Compute  the  center  of  the  object  type  in  design  coordinate 

XTC(I)  =  XTSUM/NPOLY(I) 

YTC(I)  =  YTSUM/NPOLY(I) 

ZTC(I)  =  ZTSUM/NPOLY(I) 

R_BOUND(I)  =  O.ODO 

READ  ( 9 , * )  COMMENT 

READ ( 9 , * )  NUM_SURF ( I ) 

DO  30  J  =  1,  NUM_SURF(I) 

READ  ( 9 , * )  COMMENT 

READ ( 9 , * )  NUM_CORN ( I , J ) 

READ  ( 9 , * )  COMMENT 

DO  40  K  =  1,  NUM_CORN ( I , J ) 

READ (9,*)  INDEX_CORN(I, J,K) 

40  CONTINUE 

30  CONTINUE 

Shift  the  corner  points  to  a  coordinate  system  with  it  origin  at  the 
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center  of  volume  of  the  the  obstacle  type 
DO  310  J  =  1,  NPOLY(I) 


25 


1 


XT(I,J)  =  XTD(J)  -  XTC(I) 

YT(I,J)  =  YTD(J)  -  YTC(I) 

ZT(I,J)  =  ZTD(J)  -  ZTC(I) 

R  =  SQRT(XT(I, J) **2  +  YT(I,J)**2  +  ZT(I,J)**2) 

PRINT  25,  I,  J,  XT(I,J),  YT(I,J),  ZT(I,J),R 
FORMAT ("  TYPE  =  ",  15,  "CORNER  =  ",I5,"  XT  =  ",F10.7, 
"  YT  =  ",F10.7,"  ZT  =  ",F10.7,"  R  =  ",F10.7) 


IF ( R  . GT .  R_BOUND ( I ) )  THEN 
R_BOUND ( I )  =  R 
END  IF 

310  CONTINUE 

10  CONTINUE 


READ  (  9  ,  * )  COMMENT 

READ  (  9  ,  * )  NUM_OB J 


WRITE (4, 315) 

315  FORMAT (" Ob j  #  Obj  Type  Corner  #  ", 

1  "  CX  CY  CZ "  ) 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


DO  410  1=1,  NUM_OBJ 
READ  ( 9 , * )  COMMENT 
READ ( 9 , * )  IOBJ_TYPE ( I ) 

Input  the  location  of  the  center  of  design  coordinate  system 

READ  ( 9 , * )  COMMENT 

READ (9,*)  XCI (I) , YCI (I) , ZCI (I) 

XCI (I)  =  XCI (I) 70.3048 
YCI (I)  =  YCI (I) 70.3048 
ZCI (I)  =  ZCI (I) 70.3048 


PRINT  605,  I, XCI (I) , YCI (I) , ZCI (I) 

605  FORMAT ("I  =  ",I5,"  XCI  =  ",F12.7,"  YCI  =  ",F12.7, 

1  "  ZCI  =  ",F12.7) 

Input  the  Euler  angle  to  transform  the  orientation 


C 


650 

1 


READ  (  9 , * )  COMMENT 

READ  (9,*)  PSI_E,  THETA_E,  PHI_E 
PRINT  650,PSI_E,THETA_E,PHI_E 

FORMAT ("  PSI_E  =  " , E12 . 7 , "  THETA_E  =  " , E12 . 7 , 
"  PHI_E  =  " ,E12.7) 
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Convert  from  degrees  to  radians 

E_PSI(I)  =  PSI_E* (PI/180 . 0) 
E_THETA(I)  =  THETA_E* (PI/180 . 0) 
E_PHI(I)  =  PHI_E* (PI/180 . 0) 


Checking  the  placement  of  obstacles 
K  =  IOBJ_TYPE ( I ) 


Calculate  transformation  matrix  from  object  centered  to  inertial 
oordinates 


CALL  BRYANT_MATRIX ( E_PSI ( I ) ,  E_THETA ( I ) ,  E_PHI ( I ) ,  B ) 


PRINT  755,E_PSI (I) ,  E_THETA ( I ) ,  E_PHI ( I ) 
755  FORMAT ( "E_PSI  = " , F12 . 5 , " E_THETA  =",F12.5, 

1  "E_PHI  =",F12.5) 

PRINT  607,  XCI (I) , YCI (I) , ZCI (I) 

607  FORMAT ("XCI  =  " , F12 . 7 , "  YCI  =  " , F12 . 7 , 

1  "  ZCI  =  ",F12.7) 


PRINT  315 


Transforming  the  corner  points  to  vehicle  Cartesian  and  vehicle  polar 
DO  705  J1  =  1,  NPOLY(K) 

Reoriented  corner  point  in  object  centered  coordinates 

CALL  INVERS_TRANFORMER  (B , XT (K , J1 ) , YT (K , J1 ) , ZT (K , J1 ) , 

1  XCI (I) , YCI (I) , ZCI (I) , 

1  CX,CY,CZ) 


Reoriented  corner  point  in  object  centered  coordinates 


C 


300 

705 

410 


CXE  =  CX 
CYE  =  CY 
CZE  =  CZ 

PRINT  300,  I,K, J1,CXE,CYE,CZE 
WRITE (4,300)I,K,J1, CXE , CYE , CZE 
FORMAT ( 17 , 2 ( " , " , 17 ) , 3 ( " , " , F12 . 5 ) ) 
CONTINUE 
CONTINUE 

END  IF 


XV  =  XVE 
YV  =  YVE 
ZV  =  ZVE 
C 
C 

C  Update  obstacles  in  vehicle  coordinates  to  account  for  vehicle  movement 
C 

C  Calculate  the  transformation  matrix  to  vehicle  coordinates 
CALL  BRYANT_MATRIX(Psi,  Theta,  Phi, A) 
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c 

C  Transform  object  centers  to  vehicle  Cartesian  and  polar  coordinates 
C 

CALL  VEH_COORDS (A,  RMAX,  SMAX_PHI ,  SMAX_THETA,  NUM_OBJ, 

1  XCI, YCI, ZCI,XV, YV, ZV,  R_BOUND, NUM_SEN, 

1  XCV , YCV , ZCV , lOB J_TYPE , ID , RV , VPHI , 

1  VTHETA , DELTA_ANGLE ) 

C 

C  Generate  rays  from  sensor  to  all  obstacles 
C 

CALL  SCAN  ( SMAX_PHI , SMAX_THETA, SDELTA_PHI , SDELTA_THETA, 

1  RMAX,  NUM_SEN,  XCV, YCV, ZCV, IOBJ_TYPE , ID, RV, VPHI , 

1  VTHETA, XCI, YCI, ZC I, NPOLY,  XT , YT , ZT , NUM_SURF , 

1  NUM_CORN , INDEX_CORN , DELTA_ANGLE , R_BOUND , E_PSI , 

1  E_THETA,E_PHI,XV,YV,ZV,A, 

1  SCAN_PHI , SCAN_THETA , SCAN_RANGE ) 

C 

NUM_THETA  =  INT ( SMAX_THETA/ SDELTA_THETA) *2  +  1 
C 
C 
C 

KMAX  =  INT (SMAX_PHI/SDELTA_PHI) *2  +  1 
C 
C 
C 

100  FORMAT (16) 

200  FORMAT (2E14 . 6) 

400  FORMAT (3E14 . 6) 


RETURN 

END 

C 
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SUBROUTINE  BRYANT_MATRIX ( Psi ,  Theta,  Phi, A) 

C 

C  Subroutine  calculates  the  transformation  matrix  between  coordinate 
C  system  in  term  of  three  Bryant  angles.  Assume  a  rotation  about  x  (phi) 
C  followed  by  a  rotation  about  the  transformed  y  (theta) ,  followed  by 
C  rotation  about  the  twice  transformed  z  (psi) 

C 

double  precision  A ( 3 , 3 ) , Psi , Theta , Phi , COS_PHI_l , SIN_PHI_1 , 

&  COS_THETA_2 , SIN_THETA_2 , COS_PSI_3 , 

&  SIN_PSI_3 

C 

COS_PHI_l=  COS (Phi) 

SIN_PHI_1=  SIN (Phi) 

COS_THETA_2=  COS (Theta) 

SIN_THETA_2=  SIN (Theta) 
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C0S_PSI_3=  COS(Psi) 

SIN_PSI_3=  SIN(Psi) 

A  (1,1)  =  COS_THETA_2*COS_PSI_3 

A  (1,2)  =  COS_THETA_2*SIN_PSI_3 

A  ( 1 , 3 )  =  - S IN_THETA_2 

A(2,l)  =  SIN_PHI_l*SIN_THETA_2*COS_PSI_3  -  COS_PHI_l*SIN_PSI_3 

A(2,2)  =  SIN_PHI_1*SIN_THETA_2*SIN_PSI_3  +  COS_PHI_l *COS_PSI_3 

A(2,3)  =  SIN_PHI_l*COS_THETA_2 

A(3,l)  =  COS_PHI_l*SIN_THETA_2*COS_PSI_3  +  SIN_PHI_1*SIN_PSI_3 

A(3,2)  =  COS_PHI_l*SIN_THETA_2*SIN_PHI_3  -  SIN_PHI_1 *COS_PHI_3 

A  ( 3 , 3 )  =  COS_PHI_l *COS_THETA_2 

RETURN 

END 

C 


SUBROUTINE  TRANFORMER  ( A, XI , Y1 , Z1 , XO , YO , ZO , X2 , Y2 , Z2  ) 

C 

C  Transforms  between  3D  coordinate  system  that  are  rotated  and  displaced 
C 

double  precision  A ( 3 , 3 ) , XI , Y1 , Z1 , XO , YO , ZO , X2 , Y2 , Z2 
C 

X2  =  A(l, 1) * (Xl-XO)  +  A(l, 2) * (Yl-YO)  +  A ( 1 , 3 ) * ( Zl-ZO ) 

Y2  =  A(2, 1) * (Xl-XO)  +  A(2,2) * (Yl-YO)  +  A ( 2 , 3 ) * ( Zl-ZO ) 

Z2  =  A(3 , 1) * (Xl-XO)  +  A(3 , 2) * (Yl-YO)  +  A ( 3 , 3 ) * ( Zl-ZO ) 

C 

RETURN 

END 

C 


SUBROUTINE  INVERS_TRANFORMER  ( A, XI , Y1 , Z1 , XO , YO , ZO , X2 , Y2 , Z2  ) 

C 

C  Inverse  Transformation  between  coordinate  systems  1  and  2  that 
C  are  rotated  by  the  inverse  of  matrix  A  and  displaced  to  point  0 
C 

double  precision  A ( 3 , 3 ) , XI , Y1 , Z1 , XO , YO , ZO , X2 , Y2 , Z2 
C 

X2  =  (A(1,1)*X1  +  A(2,1)*Y1  +  A(3,1)*Z1)  +  XO 

Y2  =  (A(1,2)*X1  +  A(2,2)*Y1  +  A(3,2)*Z1)  +  YO 

Z2  =  (A(1,3)*X1  +  A(2,3)*Y1  +  A(3,3)*Z1)  +  ZO 

C 

RETURN 
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END 


C 


c 

c 

c 

c 

c 


c 


c 


c 

c 

c 


SUBROUTINE  VEH_COORDS (A,  RMAX,  SMAX_PHI ,  SMAX_THETA,  NUM_OBJ, 
1  XCI, YCI, ZCI,XV, YV, ZV,  R_BOUND, NUM_SEN, 

1  XCV , YCV , ZCV , lOB J_TYPE , ID , RV , VPHI , 

1  VTHETA , DELTA_ANGLE ) 


dimension  IOBJ_TYPE (200 ) , ID (200 ) 


double  precision  XCI ( 200 ) , YCI ( 200 ) , ZCI ( 200 ) , R_Bound ( 200 ) , 
&  XCV (200), YCV (200), ZCV (200), 

&  RV(200) , VPHI (200) , 

&  VTHETA (200) ,  DELTA_ANGLE (200),A(3,3), 

&  XMV,YMV,ZMV, XV, YV,ZV, RANGE, EPSILON, 

&  DELTA_A, PI , PHI , LEFT_SCAN, RIGHT_SCAN, 

&  RATIO, THETA, UPSCAN,DOWNSCAN, RMAX, 

&  SMAX_PHI , SMAX_THETA 

DATA  PI  /3 . 1415926545/ 

DATA  EPSILON  /O. 000000001/ 

NUM_SEN  =  0 
J  =  0 

LAST_J  =  0 

DO  10  I  =  1,  NUM_OBJ 

I_OBJ  =  IOBJ_TYPE(I) 

CALL  TRANFORMER  ( A, XCI ( I ) , YCI ( I ) , ZCI ( I ) 

1  ,XV, YV, ZV,XMV, YMV, ZMV) 


Compute  the  range  to  the  Jth  corner  of  the  Ith  obstacle 
RANGE  =  SQRT(XMV**2  +  YMV* *2  +  ZMV* *2) 

Compute  the  angle  subtended  by  the  bounding  sphere  for  the  Ith  object 
IF (RANGE  .GT.  EPSILON)  THEN 

IF ( RANGE  . GT .  R_BOUND ( I_OBJ ) )  THEN 
DELTA_A  =  AS IN ( R_BOUND ( I_OB J ) / RANGE ) 

ELSE 

DELTA_A  =  PI/ 2.0 
END  IF 
ELSE 

DELTA_A  =  PI/ 2.0 
END  IF 


Check  to  see  if  the  object  bounding  sphere  is  within  sensor  range 
IF ((RANGE  -  R_BOUND ( I_OB J ) )  . LE .  RMAX)  THEN 
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PHI  =  ASIN(YMV/ (SQRT (XMV**2  +  YMV**2))) 

IF(XMV  .GE.  0.0)  THEN 

PHI  =  ASIN(YMV/ (SQRT (XMV**2  +  YMV**2))) 

ELSE 

IF(YMV  .GE.  0.0)  THEN 
PHI  =  PI  -  ASIN(YMV/ 

1  (SQRT(XMV**2  +  YMV**2))) 

ELSE 

PHI  =  -1.0*PI  -  ASIN(YMV/ 

1  (SQRT(XMV**2  +  YMV**2))) 

END  IF 
END  IF 

LEFT_SCAN  =  SMAX_PHI 
RIGHT_SCAN  =  -1 . 0 *SMAX_PHI 

RATIO  =  (YMV/ (SQRT (XMV**2  +  YMV**2))) 

Check  to  see  if  the  object  bounding  sphere  is  with  the  horizontal  scan 
IF(((PHI  -  DELTA_A) .LT.LEFT_SCAN)  .OR. 

1  ((PHI  +  DELTA_A) .GT.  RIGHT_SCAN) )  THEN 

Compute  the  pitch  angle  measure  in  the  transformed  XZ  plane  from  Z 
lockwise 

(the  XY  plan  is  at  90  degrees) 

IF (RANGE  .GT.  EPSILON)  THEN 
THETA  =  ACOS (ZMV/ RANGE) 

ELSE 

THETA  =  PI/ 2.0 
END  IF 

of  vertical  scan  in  spherical  coordinates  i.e. 
downwards 

UPSCAN  =  (PI/ 2.0)  -  SMAX_THETA 
DOWNSCAN  =  (PI/ 2.0)  +  SMAX_THETA 

the  object  bounding  sphere  is  with  the  vertical  scan 
IF(( (THETA  +  DELTA_A) .GT.  UPSCAN)  .OR. 

((THETA  -  DELTA_A) .LT. DOWNSCAN ) )  THEN 

of  objects  in  the  sensor  field  of  view 

J  =  J  +  1 
NUM_SEN  =  J 
RV(J)  =  RANGE 
VPHI(J)  =  PHI 
VTHETA(J)  =  THETA 
DELTA_ANGLE ( J)  =  DELTA_A 
XCV(J)  =  XMV 
YCV(J)  =  YMV 
ZCV(J)  =  ZMV 
IOBJ_TYPE(J)  =  I_OBJ 
ID(J)  =  I 

END  IF 
END  IF 


Calculate  limit 
C  from  the  Z  axis 


C 

C  Check  to  see  if 

1 

C 

C  Build  an  array 
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END  IF 

IF(J  .EQ.  LAST_J)  THEN 
END  IF 

LAST_J  =  J 
C 

10  CONTINUE 
RETURN 
END 
C 
C 
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SUBROUTINE  SCAN  ( SMAX_PHI , SMAX_THETA, SDELTA_PHI , SDELTA_THETA, 

1  RMAX,  NUM_SEN,  XCV, YCV, ZCV, IOBJ_TYPE , ID, RV, VPHI , 

1  VTHETA,XCI, YCI, ZCI,NPOLY,  XT , YT , ZT , NUM_SURF , 

1  NUM_CORN , INDEX_CORN , DELTA_ANGLE , R_BOUND , E_PSI , 

1  E_THETA,E_PHI,XV,YV,ZV,A, 

1  SCAN_PHI , SCAN_THETA , SCAN_RANGE ) 


C 

dimension  NPOLY (10) 

C 

C  Surfaces  in  object  types.  #  surfaces,  #  corners  in  surface,  corner  indexes 

DIMENSION  NUM_SURF ( 10 ) ,  NUM_CORN (10,10) ,  INDEX_CORN (10,10,8) 

C 

dimension  IOBJ_TYPE (200 ) , ID (200 ) 

C 

C 

C 

double  precision  SMAX_PHI , SMAX_THETA, SDELTA_PHI , RMAX, 

&  SDELTA_THETA,XT(10, 18) ,YT(10, 18) , 

&  ZT(10,18) ,XCI(200) , YCI (200) ,ZCI(200) , 

&  R_Bound (200) , E_PSI (200) , E_THETA (200), 

&  E_PHI (200) ,XCV(200) , YCV(200) , 

&  ZCV (200) ,RV(200) , VPHI (200) , VTHETA (200) , 

&  DELTA_ANGLE (200) ,A(3,3) ,B(3,3) , 

&  C(200,3,3) ,SCAN_RANGE(40,40) , 

&  SCAN_PHI(40) , SCAN_THETA(40) , PI, EPSILON, 

&  THETA, PH I, RANGE, XV, YV, ZV 

C 

DATA  PI  /3 . 1415926545/ 

DATA  EPSILON  /O. 000000001/ 

C 

NUM_PHI  =  INT (SMAX_PHI/SDELTA_PHI) *2  +  1 
NUM_THETA  =  INT ( SMAX_THETA/ SDELTA_THETA) *2  +  1 

Initialize  all  the  range  along  all  scan  ray  to  the  sensor  max 
DO  410  12  =  1,  NUM_THETA 

DO  510  J2  =  1,  NUM_PHI 
SCAN_RANGE(I2, J2)  =  RMAX 
510  CONTINUE 

410  CONTINUE 
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Vertical  scan 

THETA  = (PI/2 . 0) -1 . 0* (SMAX_THETA  +  SDELTA_THETA) 
DO  100  I  =  1,  NUM_THETA 

THETA  =  THETA  +  SDELTA_THETA 


Horizontal  scan 

PHI  =  -1 . 0* (SMAX_PHI  +  SDELTA_PHI) 
DO  200  J  =  1,  NUM_PHI 

PHI  =  PHI  +  SDELTA_PHI 


Check  object  whose  bounding  sphere  is  with  the  field  of  view 
DO  300  K  =  1,  NUM_SEN 

IF( (THETA  .LT.  (VTHETA(K)  +  DELTA_ANGLE ( K ) ) ) . AND . 

1  (THETA  .GT.  (VTHETA(K)  -  DELTA_ANGLE ( K ) ) )  .AND. 

1  (PHI  .LT.  (VPHI(K)  +  DELTA_ANGLE (K) ) ) . AND. 

1  (PHI  .GT.  (VPHI(K)  -  DELTA_ANGLE (K) ) ) )  THEN 

Ray  hits  bounding  sphere 

I_OBJ  =  IOBJ_TYPE(K) 

ID2  =  ID(K) 

CALL  SCAN_SURF (RMAX,  PHI,  THETA,  XCI(ID2),  YCI(ID2), 
1  ZCV(ID2),  I_OBJ,XV,YV,ZV,E_PSI(ID2) , 

1  E_THETA ( ID2 ) , E_PHI ( ID2 ) ,  NUM_SURF , 

1  NUM_CORN , INDEX_CORN , NPOLY ( I_OB J ) , 

1  XT, YT, ZT, A,RANGE) 

IF ( RANGE  . LT .  SCAN_RANGE ( I , J ) )  THEN 

SCAN_THETA ( I )  =  THETA 
SCAN_PHI(J)  =  PHI 
SCAN_RANGE ( I , J )  =  Range 

END  IF 


ELSE 


The  Kth  Scan  ray  miss  all  objects.  Set  range  for  the  ray  to  max 
sensor  range 

SCAN_THETA ( I )  =  THETA 
SCAN_PHI(J)  =  PHI 


300  CONTINUE 

200  CONTINUE 

100  CONTINUE 
C 

RETURN 

END 


C 

C 

SUBROUTINE  SCAN_SURF ( RMAX , SCAN_PHI , SCAN_THETA , XC , YC , 
1  ZC,  I,  XV,YV,ZV, 

1  PSI_E,THETA_E, PHI_E, 
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1  NUM_SURF , NUM_CORN , INDEX_CORN , 

1  N_POLY,XT, YT, ZT, A,RANGE) 

C 

C 

C 

C  Surfaces  in  object  types.  #  surfaces,  #  corners  in  surface,  corner 
indexes 

DIMENSION  NUM_SURF ( 10 ) ,  NUM_CORN (10,10) ,  INDEX_CORN (10,10,8) 

C 

C 

double  precision  XT ( 10 , 18 ) , YT ( 10 , 18 ) , ZT ( 10 , 18 ) , XCV ( 18 ) , 

&  YCV(18) ,ZCV(18) ,A(3,3),B(3,3) ,E_PSI(200) , 

&  E_THETA(200) ,E_PHI(200) , PI , EPSILON, RMAX, 

&  SCAN_PHI , SCAN_THETA, XC , YC , ZC , XV, YV, ZV, 

&  PSI_E,THETA_E, PHI_E, RANGE, XCR,YCR, ZCR, 

&  CX,CY,CZ, PHI_MIN, PHI_MAX,THETA_MIN, 

&  THETA_MAX , PHI , THETA , SURF_RANGE 

C 

DATA  PI  /3 . 1415926545/ 

DATA  EPSILON  /O. 000000001/ 


Calculate  transformation  matrix  from  object  centered  to  inertial 
oordinates 


CALL  BRYANT_MATRIX(PSI_E,  THETA_E,  PHI_E,  B) 


Transforming  the  corner  points  to  vehicle  Cartesian  and  vehicle  polar 
DO  100  J1  =  l,N_POLY 

Reoriented  corner  point  in  object  centered  coordinates 


1 

1 


CALL  TRANFORMER  (B , XT ( I , J1 ) , YT ( I , J1 ) , ZT ( I , J1 ) , 

0 . ODO , 0 . ODO , 0 . ODO , 

XCR, YCR, ZCR) 


Translate  corner  point  to  new  location  in  initerial  coordinates  system 
CX  =  XCR  +  XC 
CY  =  YCR  +  YC 
CZ  =  ZCR  +  ZC 


Transforming  the  corner  points  from  initial  to  vehicle  Cartesian 
CALL  TRANFORMER  (A,CX,CY,CZ, 

1  XV,YV,ZV, 

1  XCV(Jl) , YCV(Jl) , ZCV(Jl) ) 

C 

100  CONTINUE 

C  Checking  each  surface  to  see  if  the  ray  passes  through  it 
RANGE  =  RMAX 
C 

DO  200  J  =  1,  NUM_SURF(I) 

C 

PHI_MIN  =  PI 
PHI_MAX  =  -1.0 *PI 
THETA_MIN  =  PI 
THETA_MAX  =0.0 
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c 


DO  300  K  =  1,  NUM_CORN(I, J) 

C  Compute  the  range  to  the  Kth  corner  of  the  Jth  surface  of  the  Ith  object 
L  =  INDEX_CORN ( I , J , K ) 


Compute  the  yaw  angle  measure  in  the  XY  plan  counter  clockwise  from  X 
IF(XCV(L)  .GE.  0.0)  THEN 

PHI  =  ASIN(YCV(L) / (SQRT (XCV(L) **2  +  YCV(L)**2))) 

ELSE 

IF(YCV(L)  .GE.  0.0)  THEN 
PHI  =  PI  -  ASIN(YCV(L) / (SQRT (XCV(L) **2 
1  +  YCV(L) **2) ) ) 

ELSE 

PHI  =  -1.0* (PI)  -  ASIN(YCV(L)/ 

1  (SQRT (XCV(L) **2  +  YCV(L)**2))) 

END  IF 
END  IF 


Check  for  max  and  min  horizontal  angle 

IF (PHI  .LT.  PHI_MIN)  THEN 
PHI_MIN  =  PHI 
END  IF 

IF (PHI  .GT.  PHI_MAX)  THEN 
PHI_MAX  =  PHI 
END  IF 


Compute  the  pitch  angle  measure  in  the  transformed  XZ  plane  from  Z 
lockwise 

(the  XY  plan  is  at  90  degrees) 

THETA  =(PI/2)-  ASIN(ZCV(L) / (SQRT (XCV(L) **2  +  YCV(L)**2  + 
1  ZCV(L)**2))) 

Check  for  max  and  min  horizontal  angle 

IF (THETA  .LT.  THETA_MIN)  THEN 

THETA_MIN  =  THETA 
END  IF 

IF (THETA  .GT.  THETA_MAX)  THEN 
THETA_MAX  =  THETA 
END  IF 

300  CQNTINUE 

IF  ((SCAN_PHI  .LT.  PHI_MAX)  .AND.  (SCAN_PHI.  GT .  PHI_MIN) 
1  .AND.  (SCAN_THETA  . LT .  THETA_MAX)  .AND. 

1  (SCAN_THETA.  GT .  THETA_MIN) )  THEN 

Compute  the  intersection  of  the  scan  ray  with  the  surface 

CALL  RAY_INTERSECT (RMAX,  SCAN_THETA,  SCAN_PHI ,  I,  J, 

1  NUM_CQRN,  INDEX_CQRN,  XCV,  YCV,  ZCV, 

1  SURF_RANGE ) 

IF  (SURF_RANGE  . LT .  RANGE)  THEN 
RANGE  =  SURF_RANGE 

END  IF 
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c 


END  IF 

200  CONTINUE 
C 

RETURN 

END 

C 


SUBROUTINE  RAY_INTERSECT (RMAX,  SCAN_THETA,  SCAN_PHI ,  I,  J, 

1  NUM_CORN,  INDEX_CORN,  XCV,  YCV,  ZCV, 

1  RANGE) 


C 

C  Surfaces  in  object  types.  #  surfaces,  #  corners  in  surface,  corner 
indexes 


C 

C 


DIMENSION  NUM_SURF ( 10 ) ,  NUM_CORN (10,10) ,  INDEX_CORN (10,10,8) 


double  precision 

& 

& 

& 

& 

& 


XCV (18) , YCV (18) , ZCV (18) , EPSILON, RMAX, 
SCAN_THETA, SCAN_PHI , RANGE , A1 , A2 , A3 , B1 , 
B2 , B3 , Cl , C2 , C3 , DV, A, B, C , DP, XUSR, YUSR, 
ZUSR , COS_PS I , DSR , XSR , YSR , Z  SR , XSUM , YSUM , 
ZSUM ,  XSC  ,  YSC  ,  ZSC  ,  AA ,  AB ,  AC  ,  UO  ,  VO  ,  U1 ,  VI , 
U2 , V2 , DO , D1 , D2 , ALPHA , BETA , ABSUM 


LOGICAL  FOUND 


DATA  EPSILON  /O. 000000001/ 
C 


KO 

= 

INDEX_CORN ( I , J , 1 ) 

K1 

= 

INDEX_CORN ( I , J , 2 ) 

K3 

= 

INDEX_CORN ( I , J , NUM_CORN ( I , 

Define 

the  vector  that  make  up  the 

A1 

= 

XCV(Kl)  -  XCV(KO) 

A2 

= 

YCV(Kl)  -  YCV(KO) 

A3 

= 

ZCV(Kl)  -  ZCV(KO) 

B1 

= 

XCV(K3)  -  XCV(KO) 

B2 

= 

YCV(K3)  -  YCV(KO) 

B3 

= 

ZCV(K3)  -  ZCV(KO) 

Defining 

the  vector  cross  product. 

Cl 

= 

A2*B3  -  A3*B2 

C2 

= 

A3*B1  -  A1*B3 

C3 

= 

A1*B2  -  A2*B1 

J)  ) 

edges  of  the  surface 


Vector  will  be  normal  to  the  surface 


Calculate  the  length  of  the  vector 

DV  =  SQRT(C1**2.0  +  C2**2.0  +  C3**2.0) 

Elements  of  the  unit  normal  vector  to  the  plan  of  the  surface 
A  =  Cl/DV 


48 


oo  ooooooo  oo  oo  ooooooo 


B  =  C2/DV 
C  =  C3/DV 

C  Calculating  the  distance  from  the  coordinate  center  (vehicle  center)  to 
the 

C  plane  containing  the  surface. 

DP  =  ABS (A*XCV(K1)  +  B*YCV(K1)  +  C*ZCV(K1)) 

Calculating  the  elements  of  a  unit  vector  parallel  with  the  scan  ray 
XUSR  =  SIN(SCAN_THETA) *COS (SCAN_PHI) 

YUSR  =  SIN(SCAN_THETA) *SIN(SCAN_PHI) 

ZUSR  =  COS (SCAN_THETA) 

For  unit  vectors  starting  at  the  origin  the  direction  cosines  are  equal 
to  the  elements  of  the  vector.  Using  the  direction  cosines,  the  solid 
angle  between  the  surface  normal  and  the  scan  ray  is  the  following 
COS_PSI  =  ABS((A*XUSR  +  B*YUSR  +  C*ZUSR) ) 

Distance  from  the  vehicle  to  the  plane  containing  the  surface  along 
the  scan  ray 

IF(COS_PSI  .GT.  EPSILON)  THEN 
DSR  =  DP/COS_PSI 
ELSE 

DSR  =  RMAX 
END  IF 

Point  at  which  the  scan  ray  intersect  the  plane  containing  the  surface 
XSR  =  DSR*XUSR 
YSR  =  DSR*YUSR 
ZSR  =  DSR*ZUSR 

Finding  the  center  of  the  surface 
XSUM  =0.0 
YSUM  =0.0 
ZSUM  =0.0 

DO  100  K  =  1,  NUM_CORN ( I , J ) 

L  =  INDEX_CORN(I, J,K) 

XSUM  =  XCV(L)  +  XSUM 
YSUM  =  YCV(L)  +  YSUM 
ZSUM  =  ZCV(L)  +  ZSUM 
100  CONTINUE 

XSC  =  XSUM/NUM_CORN(I, J) 

YSC  =  YSUM/NUM_CORN(I, J) 

ZSC  =  ZSUM/NUM_CORN(I, J) 

Divide  the  surface  in  triangles .  Each  triangle  contains  two  corner  points 
and  the  center  point.  Check  each  triangle  to  see  if  the  point  where  the 
scan  ray  contacts  the  plane  containing  the  surface  is  inside. 

Check  each  triangle 

FOUND  =  . FALSE . 

DO  200  K  =  1,  NUM_CORN ( I , J ) 

L  =  INDEX_CORN(I, J,K) 

KPl  =  K  +  1 

IF(KP1  .GT.  NUM_CORN ( I , J ) )  THEN 
KPl  =  1 
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c 


c 


c 


c 


c 

c 


c 


c 


c 

c 


END  IF 

M  =  INDEX_CORN(I, J,KP1) 

AA  =  ABS(A) 

AB  =  ABS(B) 

AC  =  ABS(C) 


(  (AC 

.GE. 

AA)  .AND 

UO 

= 

XSR  - 

XSC 

VO 

= 

YSR  - 

YSC 

U1 

= 

XCV ( L ) 

-  XSC 

VI 

= 

YCV ( L ) 

-  YSC 

U2 

= 

XCV(M) 

-  XSC 

V2 

= 

YCV(M) 

-  YSC 

:  IF ( (AB  .GE 

;.  AA)  .. 

UO 

= 

XSR  - 

XSC 

VO 

= 

ZSR  - 

zsc 

U1 

= 

XCV ( L ) 

-  XSC 

VI 

= 

ZCV(L) 

-  zsc 

U2 

= 

XCV(M) 

-  XSC 

V2 

= 

ZCV(M) 

-  zsc 

;e 

UO 

= 

YSR  - 

YSC 

VO 

= 

ZSR  - 

zsc 

U1 

= 

YCV ( L ) 

-  YSC 

VI 

= 

ZCV(L) 

-  zsc 

U2 

= 

YCV(M) 

-  YSC 

V2 

= 

ZCV(M) 

-  zsc 

(AC  .GE.  AB) )  THEN 


(AB  .GE.  AC))  THEN 


END  IF 

DO  =  U1*V2  -  U2*V1 
D1  =  U0*V2  -  U2*V0 
D2  =  U1*V0  -  U0*V1 

ALPHA  =  Dl/DO 

BETA  =  D2/D0 

ABSUM  =  ALPHA  +  BETA 


IF ((ALPHA  .GE.  0.0)  .AND. 
1  (ABSUM  .LE.  1.0))  THEN 


(BETA  .GE.  0.0)  .AND. 


FOUND 


. TRUE . 


END  IF 


C 

200  CONTINUE 


IF (FOUND)  THEN 
RANGE  =  DSR 
ELSE 

RANGE  =  RMAX 
END  IF 
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RETURN 

END 


c 


c 


SUBROUTINE  Huinan_Driver  (NPOWER , RMAX , SMAX_PHI , 

1  SMAX_THETA , SDELTA_PHI , 

&  SDELTA_THETA, 

&  X_Sen,  Y_Sen,  Z_Sen, 

&  Psi_Sen, Theta_Sen, Phi_Sen) 


double  precision  RMAX, SMAX_PHI , SMAX_THETA, SDELTA_PHI , 

&  SDELTA_THETA, GRAY, TICYCL, TSS , DMAX, XP, 

&  YP,TAUMEM,TFF,RM, A,B,RI, PSIO,TLAST, 

&  DFWLST,TILAST,DMEM,XT, YT, 

&  CAF,CAR,WHBS,WF,WR,U,X_Sen, 

&  Y_Sen,  Z_Sen, Psi_Sen, Theta_Sen, Phi_Sen 

C 

C - COMMON  blocks - 

C 

COMMON  /DRVSTl/  GRAY,  TICYCL,  TSS,  DMAX,  XP(IOO),  YP(IOO),  TAUMEM, 

1  TEE,  RM,  A,  B,  RI ,  PS 10,  NTF,  NP,  TLAST,  DFWLST,  TILAST, 

2  DMEM(1000,2) ,  XT(IOO),  YT(IOO) 

SAYE/DRYSTl/ 

COMMON  /DRIY/  CAE,  CAR,  WHBS ,  WE,  WR,  U 
SAYE/DRIY/ 

COMMON  /INOUT/  R,  W 
SAYE/INOUT/ 


C 

C - COMMON  Yariables - 

C  R . Driver  Model  Input  I/O  unit  ( " DMINPUT . INP " ) 

C  W . Driver  Model  Output  I/O  unit  ( "DMOUTPUT . OUT" ) 

C 

C - DRIY.BLK  common  block  variables 

C 

C  CAE... total  cornering  stiffness  of  tires  on  left  front  susp  (Ib/rad) 
C  CAR... total  cornering  stiffness  of  tires  on  left  rear  susp  (Ib/rad) 

C  WHBS . .wheelbase  of  vehicle  (center-line  of  front  &  rear  susp)  (ft) 

C  WE.... static  load  on  front  suspension  (lb) 

C  WR.... static  load  on  rear  suspension  (lb) 

C  U . initial  velocity  (ft/sec) 


- DRYSTl.BLK  common  block  variables - 

C234567 890 1234567 890 1234567 890 1234567 890 1234567 890 1234567 89012 3456789012 


C  GRAY . gravitational  constant 

C  TICYCL ...  driver  model  sample  time  (sec) 

C  TSS . minimum  preview  time  (sec) 

C  DMAX . upper  bound  on  front  wheel  angle  steer  (rad) 

C  XP,  YP...X-Y  path  coords(SAE)  wrt  inertial  coords  (input)  (ft) 

C  TAUMEM. .. driver  transport  time  dealy  (input  parameter)  (sec) 

C  TEE . driver  model  preview  time  (input  parameter)  (sec) 

C  RM . vehicle  mass  (slug) 

C  A . distance  from  c.g.  to  front  suspension  center-line  (ft) 

C  B . distance  from  c.g.  to  rear  suspension  center-line  (ft) 
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C  RI . total  vehicle  yaw  inertia  (slug-ft) 

C  PSIO . current  yaw  angle  reference  value  (rad) 

C  NTF . number  of  points  in  the  preview  time  interval 

C  NP . number  of  points  in  the  x-y  trajectory  table 

C  TLAST . . . . last  time  driver  model  calculated  a  steer  value  (sec) 

C  DFWLAST . . last  value  of  steer  calculated  by  driver  model  (rad) 

C  TILAST . . . last  sample  time  driver  model  calculated  a  steer  value  (sec) 

C  DMEM . 2-dim  array  (time  &  steer  history)  used  in  delay  calculat'n 

C  XT,  YT ...  transformation  of  XP,YP  in  vehicle  body  axes  (ft) 

C 

C - Local  variables - 

C 

CHARACTER* 80  COMMENT 
C 

C  Set  Pi 

DATA  PI  /3 . 1416/ 

C  Set  Acceleration  of  gravity  (ft/sec**2) 

DATA  GRAV  /32. 16666/ 

C  Set  driver  model  sample  time 
C  DATA  TICYCL  /O. 000001/ 

data  ticycl  /l.OD-6/ 

C  Set  minimum  driver  model  preview  time 
DATA  TSS  /O . ODO/ 

C  Set  upper  bound  on  front  wheel  steering  angle 


DMAX  =0.2 


OPEN (UNIT=3 , FILE= ' Human_Driver . txt ' ) 
REWIND  3 


READ ( 3 , * )  COMMENT 

READ ( 3 , * )  RMAX , ACCR ,  SMAX_PHI ,  SMAX_THETA 


Convert  to  ft 

RMAX  =  RMAX/ 0.3 048 

Convert  to  radians 

SMAX_PHI  =  SMAX_PHI* (PI/180 . ) 
SMAX_THETA  =  SMAX_THETA* ( PI / 180 . ) 

C 

READ ( 3 , * )  COMMENT 
C 

READ ( 3 , * )  SDELTA_PHI ,  SDELTA_THETA 

SDELTA_PHI  =  SDELTA_PHI* (PI/180 . ) 
SDELTA_THETA  =  SDELTA_THETA* ( PI / 180 . ) 
C 

READ ( 3 , * )  COMMENT 
C 

READ ( 3 , * )  NPOWER 
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READ ( 3 , * )  COMMENT 
C 

READ (3,*)  X_Sen,  Y_Sen,  Z_Sen 
C 

READ ( 3 , * )  COMMENT 
C 

READ ( 3 , * )  Ps i_Sen , Theta_Sen , Phi_Sen 
C 

Psi_Sen  =  Psi_Sen* (PI/180 . ) 

Theta_Sen  =  Theta_Sen* (PI/180 . ) 

Phi_Sen  =  Phi_Sen* (PI/180 . ) 

C 

DO  90  I  =  1,  1000 

DMEM (1,1)  =  0 . 

DMEM(I,2)  =  -1. 

90  CONTINUE 
C 

RETURN 

END 

C 

C 

icifitifififirifififitifitifirififiritifififirifitifififitifirififiritif-k-k-k'k-k-k-k-k-k-k-k'k-k-k-k-k-k-k-k'k-k-k-kic'k'k'k'k'k'k'k'k 


c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  HUMAN_STEERING (T , Tdelta , Y , DEW, DFWNOW, 

&  XSTAR,YSTAR) 

double  precision  T , Y ( 5 ), DEW, DFWNOW, XSTAR, YSTAR, GRAY, TICYCL, 

&  TSS,DMAX,XP, YP,TAUMEM,TFF,RM, A,B,RI, PSIO, 

&  TLAST , DFWLST , T I LAST , DMEM, XT , YT , AAA, BBB, 

&  CCC,DDD, RATIO, Tdelta 

COMMON  /DRVSTl/  GRAY, TICYCL , TSS , DMAX , XP ( 10 0 ) , YP ( 10 0 ) , TAUMEM, 

1  TEE , RM , A , B , RI , PSI 0 , NTF , NP , TLAST , DFWLST , TILAST , 

2  DMEM(1000,2) ,  XT(IOO),  YT(IOO) 

SAYE/DRYSTl/ 


COMMON  /YEHTYP/  AAA,  BBB,  CCC ,  DDD,  RATIO 
SAYE/YEHTYP/ 

LOGICAL  FIRST 
CHARACTER* 80  COMMENT 

DATA  FIRST  /.TRUE./ 

IF (FIRST)  THEN 

OPEN  (UNIT=5  ,  FILE=  '  Huinan_Steering  .  txt '  ) 
REWIND  5 
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c 


READ ( 5 , * )  COMMENT 


READ ( 5 , * )  TAUMEM 
READ ( 5 , * )  COMMENT 
READ ( 5 , * )  TEE 
READ ( 5 , * )  COMMENT 
READ (5,*)  TSS 
READ ( 5 , * )  COMMENT 
READ ( 5 , * )  DFWNOW 
READ ( 5 , * )  COMMENT 
READ ( 5 , * )  AAA 
READ ( 5 , * )  COMMENT 
READ ( 5 , * )  BBS 
READ ( 5 , * )  COMMENT 
READ ( 5 , * )  CCC 
READ ( 5 , * )  COMMENT 
READ ( 5 , * )  DDD 
READ ( 5 , * )  COMMENT 
READ (5,*)  RATIO 
PRINT  75,  TAUMEM,  TEE 

FORMAT  ('  /,  '  T20,  'DRIVER  TRANSPORT  LAG  (SEC) 

1  F4 . 2 T20 ,  'END  OF  PREVIEW  INTERVAL  (SEC) 

2  F4.2/) 

NTF  =  10 
TLAST  =  0 . ODO 
DFWLAST  =  0 . ODO 
TILAST  =  0 . ODO 
DEW  =  0 . ODO 
FIRST  =  .FALSE. 

END  IF 

DO  10  I  =  1,  5 

write (3 8,*)  "I  =  ",I,"  Y  =  ",Y(I) 

CONTINUE 


:  ' ,  T60, 
:  ' ,  T60, 
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CALL  NEWDRIVER(T,  Y,  DEW,  DFWNOW , XSTAR , YSTAR ) 

C 

DFWNOW  =  DEW 

RETURN 

END 

iciritifififirififirififitifirifitiritifitifirififirififitifirifitifififififirifif-k-k-k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k'k'k'k'k'k'k'k'k'k'k'k'k 

c 

i(  if  i(  i<  if  i(  -k  -k  -k  -k  -k  -k  -k  ic  it  ic  it  ic  it  ic  it  ic  it  ic  ic  ic 


c 

C  ***  Closed-Loop  Steer  Calculation  Subroutine  *** 

C 

C  NEWDRIVER:  Computes  closed-loop  steering  control  during  the  simulation. 
C  NEWDRIVER  is  a  modification  of  the  original  DRIVER 

C  subroutine  described  below. 

C 

C 

C - Author  and  Modification  Section - 


C 

C  Author :  C .  C .  MacAdam 

C 

C  Date  written:  01/01/88 
C 

C  Written  on: 

C 

C  Modifications: 

C 

C - 

C 

C - Algorithm  Description - 

C 

C  Purpose  and  use: 

C 

C  Error  conditions: 

C 

C  References : 

C 

C  (1)  MacAdam,  C.  C.  "Development  of  Driver/Vehicle  Steering 

C  Interaction  Models  for  Dynamic  Analysis,"  Final 

C  Technical  Report,  U.S.  Army  Tank  Automotive  Command 

C  Contract  No.  DAAE07-85-C-R069 ,  The  University  of 

C  Michigan  Transportation  Research  Inst.  December  1,  1988 

C 

C  (2)  MacAdam,  C.  C.  "Application  of  an  Optimal  Preview  Control 

C  for  Simulation  of  Closed-Loop  Automobile  Driving, " 

C  IEEE  Transactions  on  Systems,  Man,  and  Cybernetics, 

C  Vol.  11,  June  1981. 

C 

C  (3)  MacAdam,  C.  C.  "An  Optimal  Preview  Control  for  Linear 

C  Systems,"  Journal  of  Dynamic  Systems,  Measurement, 

C  and  Control,  ASME,  Vol.  102,  No.  3,  September  1980. 

C 

C 

C  Machine  dependencies:  none 


55 


o  o 


c 

C  Called  by:  STEERING.! 

C 

C - 

C 

SUBROUTINE  NEWDRIVER(X,  Y,  DEW,  DFWNOW , XSTAR , YSTAR ) 


C 

C - Variable  Descriptions - 

C 

C - Arguments  passed: 

C 

C  ->  X . time  in  the  simulation  (sec) 

C  ->  Y . current  driver  model  state  vector  obtained  from  DADS. 

C  Driver  model  state  vector  of  dimension  5  comprised  of  the 

C  following  physical  quantities:  (1)  inertial  lateral 

C  displacement  (ft),  (2)  lateral  veloc  in  body  frame  (ft/s), 

C  (3)  yaw  rate  global  (rad/s),  (4)  SAE  global  yaw  angle  (rad), 

C  (5)  global  forward  displacement  (ft). 

C 

C  <-  DEW.  ...  closed-loop  steering  control  returned  to  DADS  (returned) 

C  ->  DFWNOW . current  steering  angle  (average)  of  front  wheels,  passed 
C  in  after  effects  of  roll-steer,  compliance,  etc. 

C  XSTAR  -  Previewed  or  desired  longitudinal  position 
C  YSTAR  -  Previewed  or  desired  lateral  position 


INTEGER  R,  W 
C 

double  precision  Y ( 5 ) , YC ( 5 ) , DUMVll ( 4 ) , DUMVl ( 4 ) , VECM ( 4 ) , 

&  DUMMl (4,4) ,DUMM2 (4,4) , X , DEW , DFWNOW , 

&  XSTAR , YSTAR , GRAV , TICYCL , TSS , DMAX , 

&  XP, YP,TAUMEM,TFF,RM, A,B,RI, PSIO,TLAST, 

&  DFWLST , TILAST , DMEM, XT , YT , CAE , CAR , WHBS , 

&  WF,WR,U,TTT,TTT1,G,AAA,BBB,CCC, 

&  DDD , RATIO , CCAFl , CCAF2 , CCARl , CCAR2 , 

&  FFZLl , FFZL2 , FFZL3 , FFZL4 , DMVELC , PI , 

&  ANGLE , DELTA_ANGLE ,XV,YV,T,EPSI,Y0,X0, 

&  EPSY2 , TSUM, SSUM, CAFTEM, CARTEM, TJI , XCAR, 

&  YPATH,S1,EP,T1,TTAB,DFWLAST 

C 

C - COMMON  blocks - 

C 

COMMON  /DRVSTl/  GRAV,  TICYCL,  TSS,  DMAX,  XP(IOO),  YP(IOO),  TAUMEM, 

1  TEE,  RM,  A,  B,  RI ,  PSIO,  NTF,  NP,  TLAST,  DFWLST,  TILAST, 

2  DMEM(1000,2) ,  XT(IOO),  YT(IOO) 

SAVE /DRVSTl/ 

C 

COMMON  /DRIV/  CAE,  CAR,  WHBS,  WE,  WR,  U 
SAVE/DRIV/ 

C 

COMMON  /INOUT/  R,  W 
SAVE/INOUT/ 

C 

COMMON  /TRSSTR/  TTT(4,4,10),  TTTl ( 4 , 4 , 10 ) ,  G(4) 

SAVE  /TRSSTR/ 

C 

COMMON  /VEHTYP/  AAA,  BBB,  CCC ,  DDD,  RATIO 
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SAVE/VEHTYP/ 


C 

C  Get  Tire  Cornering  Stiffnesses,  Vertical  Tire  Loads,  and  Speed 
C  from  DADS  Through  Common  Block  DMTIR 
C 

COMMON/DMTIR/CCAFl , CCAF2 , CCARl , CCAR2 , FFZLl , FFZL2 , FFZL3 , FFZL4 , 
+  DMVELC 
C 
C 

C - COMMON  Variables - 

C  R . Driver  Model  Input  I/O  unit  ( " DMINPUT . INP " ) 

C  W . Driver  Model  Output  I/O  unit  ( "DMOUTPUT . OUT" ) 

C 

C - DRIV.BLK  common  block  variables - 

C 

C  Initial  Values  from  Time  Zero: 


C 

C  CAF... total  cornering  stiffness  of  tires  on  left  front  susp  (Ib/rad) 
C  CAR... total  cornering  stiffness  of  tires  on  left  rear  susp  (Ib/rad) 

C  WHBS .. wheelbase  of  vehicle  (center-line  of  front  &  rear  susp)  (ft) 

C  WF.... static  load  on  front  suspension  (lb) 

C  WR.... static  load  on  rear  suspension  (lb) 

C  U . initial  velocity  (ft/sec) 

C 


C - DMTIR. BLK  common  block  variables - 

C 

C  Updates  during  simulation  run: 

C 

C  CCAFl...Left  front  tire  cornering  stiffness  from  DADS  during  run 
C  CCAF2 ...  Right  front  tire  cornering  stiffness  from  DADS  during  run 
C  CCARl... Left  rear  tire  cornering  stiffness  from  DADS  during  run 
C  CCAR2 ...  Right  rear  tire  cornering  stiffness  from  DADS  during  run 
C  FFZLl. . .Left  front  tire  vertical  load  from  DADS  during  run 
C  FFZL2 . . .Right  front  tire  vertical  load  from  DADS  during  run 
C  FFZL3 . . .Left  rear  tire  vertical  load  from  DADS  during  run 
C  FFZL4 . . .Right  rear  tire  vertical  load  from  DADS  during  run 
C  DMVELC . . Forward  speed  from  DADS 
C 

C - DRVSTl.BLK  common  block  variables - 


C 

C  GRAV. . . .gravitational  constant 
C  TICYCL .. driver  model  sample  time  (sec) 

C  TSS . minimum  preview  time  (sec) 

C  DMAX. . . .upper  bound  on  front  wheel  angle  steer  (rad) 

C  XP,YP...x-y  path  coords(SAE)  wrt  inertial  coords  (input)  (ft) 

C  TAUMEM. . driver  transport  time  dealy  (input  parameter)  (sec) 

C  TFF . driver  model  preview  time  (input  parameter)  (sec) 

C  RM . vehicle  mass  (slug) 

C  A . distance  from  c.g.  to  front  suspension  center-line  (ft) 

C  B . distance  from  c.g.  to  rear  suspension  center-line  (ft) 

C  RI . total  vehicle  yaw  inertia  (slug-ft) 

C  PSIO ....  current  yaw  angle  reference  value  (rad) 

C  NTF . number  of  points  in  the  preview  time  interval 

C  NP . number  of  points  in  the  x-y  trajectory  table 

C  TLAST...last  time  driver  model  calculated  a  steer  value  (sec) 

C  DFWLST..last  value  of  steer  calculated  by  driver  model  (rad) 

C  TILAST..last  sample  time  driver  model  calculated  a  steer  value  (sec) 
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c 

c 

c 

c- 

c 

c 

c 

c 

c 

c- 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c- 

c 

c 

c- 

c 

c- 

c 

c 


DMEM. . . . 2-diin  array  (time  &  steer  history)  used  in  a  delay  calculat'n 
XT , YT ...  transformation  of  XP,YP  in  vehicle  body  axes  (ft) 

--TRSSTR.BLK  common  block  variables - 

TTT . transition  matrix  at  10  discrete  points  in  preview  interval 

TTTl . . . .integral  of  trans  matrix  wrt  preview  time 
G . vector  of  control  gain  coefficients 


--Local  variables 


YC . local  (body-axis  based)  copy  of  state  vector  Y 

VECM. ...  observer  vector  -  lateral  displacement  from  state  vector 

DUMVl . . .work  vector 

DUMVll . . work  vector 

DUMMl . . .work  matrix 

DUMM2 . . .work  matrix 

T . time  in  the  simulation  (sec) 

EPSI. . . .yaw  angle  between  body  axis  and  current  index  value,  PSIO 
PSIO ....  current  nominal  value  of  yaw  angle  used  for  linearization 

NP . number  of  points  in  x-y  path  table 

XP,YP...x-y  inertial  path  table  (input)  (ft) 

XT,YT...x-y  path  table  transformed  to  body  axis  (PSIO)  system  (ft) 

EPSY2 ...  cumulative  preview  path  error  squared 

EPSI. . . .mean  squared  value  of  cumulative  preview  path  error 

TSUM. . . .scalar  work  quantity 

SSUM. . . .scalar  work  quantity 

DFWLAST . steering  control  from  last  calculation  (rad) 

TJI . preview  time  ahead  from  present  time  value  (sec) 

I , J , K ...  integer  counters 

XCAR. .. .preview  distance  ahead  in  feet  (ft) 

XO . present  forward  position  of  vehicle  c.g.  (ft) 

TTAB . . . .current  time  less  the  driver  delay,  TAUMEM.  Used  to  access 
the  delayed  driver  response  stored  in  DMEM  array.  (sec) 

SI . scalar  work  quantity 

T1 . scalar  work  quantity 

EP . previewed  path  error  (ft) 


--Functions  and  Subroutines 
EXTERNAL  GMPRD 


Process  Block 


DATA  VECM  / 1 . ODO , 0 . ODO , 0 . ODO , 0 . ODO / 
DATA  PI  /3 . 141592/ 

DATA  ANGLE/1.57079/ 

DATA  DELTA_ANGLE/0 . 00628319/ 


NP=1 

XP(1)  =  XSTAR  -  XV 
YP ( 1 ) =YSTAR  -  YV 
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T  =  X 

c 

EPSI  =  ABS(Y(4)  -  PSIO) 

C 

DO  10  I  =  1,  5 
C 

10  YC(I)  =  Y(I) 

Update  Coordinate  Transformation 


PSIO  =  Y(4) 

DO  20  J  =  1,  NP 


XT  (J) 

=  XP(J)  * 

CQS (PSIQ) 

+  YP(J)  * 

SIN(PSIQ) 

20 

YT  ( J) 

=  -XP(J)  * 

SIN(PSIQ) 

+  YP(J)  * 

CQS (PSIQ) 

30 

YO  = 

-Y(5) 

*  SIN(PSIQ) 

+  Y(l)  * 

CQS (PSIQ) 

XO  = 

Y(5) 

*  CQS(PSIQ) 

+  Y(l)  * 

SIN(PSIQ) 

YC  (1) 

=  YO 

YC(4)  =  Y(4)  -  PSIO 
EPSY2  =  O.ODO 
TSUM  =  0 . ODO 
SSUM  =  O.ODO 

SET  DEW  EQUAL  TO  THE  PREVIOUS  STEERING  COMMAND 
C 

DEW  =  DFWLST 
C 

C  Return  if  time  from  last  calculation  less  than  sample  interval 
C 

IF  (T  -  TILAST  .LE.  TICYCL)  RETURN 
C  if ( t-tilast . le . ticycl ) then 

C  return 

C  endif 

C 

C  THE  NEXT  6  LINES  OF  EXECUTABLE  CODE  MAY  BE  COMMENTED  OUT  TO  BYPASS 
C  CONTINUOUS  UPDATING  OF  THE  TRANSITION  MATRICES,  IF  NOT  REQUIRED. 

C  SEE  SECTIQN  5 . 8  QF  REFERENCE  1 . 

C 

C  UPDATE  TIRE  CQRNERING  STIFFNESSES  AND  VEHICLE  VELQCITY 
C  AND  RECALCULATE  TRANSITIQN  MATRIX: 

C 

C 

CAFTEM  =  (CCAF1*FFZL1+CCAF2*FFZL2)  /  ( FFZL1+FFZL2 ) 

CARTEM  =  (CCAR1*FFZL3+CCAR2*FFZL4)  /  ( FFZL3 +FFZL4 ) 

CAE  =  CAFTEM 
CAR  =  CARTEM 
C 
C 

C  UPDATE  TRANSITIQN  MATRICES 

C 

C 

CALL  TRANS 
C 
C 

C  LQQP  TQ  CALCULATE  QPTIMAL  PREVIEW  CQNTRQL  PER  REFERENCES  2  &  3 : 

C  (NTF  PQINTS  WITHIN  THE  PREVIEW  INTERVAL) 

C 
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DO  50  I  =  1,  NTF 

TJI  =  (TFF  -  TSS)  /  NTF  *  I  +  TSS 
DO  40  J  =  1,  4 

DO  40  K  =  1,  4 

DUMM1(J,K)  =  TTT1(J,K,I) 

DUMM2(J,K)  =  TTT(J,K,I) 

40  CONTINUE 

C 

CALL  GMPRD(VECM,  DUMMl ,  DUMVll,  1,  4,  4) 

CALL  GMPRD(VECM,  DUMM2 ,  DUMVl ,  1,  4,  4) 

CALL  GMPRD (DUMVl,  YC ,  T1 ,  1,  4,  1) 

C 

C  Get  observed  path  input,  YPATH,  within  preview  interval  at  XCAR  ft 
C 

XCAR  =  XO  +  U  *  TJI 
C 

YPATH=YT ( 1 ) 

C 

CALL  GMPRD (DUMVll,  G,  SI,  1,  4,  1) 

C 

C  EP  is  the  previewed  path  error  at  this  preview  point 
C 

EP  =  T1  +  SI  *  DFWNOW  -  YPATH 
C 
C 

TSUM  =  TSUM  +  EP  *  SI 
SSUM  =  SSUM  +  SI  *  SI 

Cumulative  preview  error  calculation  (unrelated  to  control) 

EPSY2  =  EPSY2  +  EP  *  EP  *  (TFF  -  TSS)  /  NTF 
50  CONTINUE 

Cumulative  preview  error  calculation  (  unrelated  to  control) 

EPSY2  =  SQRT(EPSY2)  /  (TFF  -  TSS) 

C 

C  Optimal  value  -  no  delay  yet. 

C 

DFW  =  -TSUM  /  SSUM  +  DFWNOW 
C 
C 

C  Maximum  steer  bound  set  at  DMAX  (arbitrary) 

C 

If  (ABS(DFW)  .GT.  DMAX)  THEN 
IF  (DFW  .LT.  0.0)  THEN 
DFW  =  -1.0*DMAX 
ELSE 

DFW  =  DMAX 
END  IF 
END  IF 
C 
C 

C  Store  steer  history  and  corresponding  times  in  DMEM. 

C  Retrieve  steer  delayed  by  TAUMEM  sec  and  return  as 
C  delayed  driver  steer  control,  DFW. 
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DO  60  J  =  1,  2 

DO  60  I  =  1,  999 

60  DMEM(1001  -  I,J)  =  DMEM(1000  -  I,J) 

DMEM (1,1)  =  DEW 
DMEM (1,2)  =  T 
TTAB  =  T  -  TAUMEM 
C 

DO  70  I  =  1,  999 
C 

IF  (DMEM (I  +1,2)  .LE.  TTAB  .AND.  DMEM ( I , 2 )  . GE .  TTAB) 

1  GO  TO  90 

70  CONTINUE 

WRITE  (W, 80) TAUMEM, DEW, X 

80  FORMAT  ('O',  '*****  TAUMEM  PROBABLY  TOO  LARGE  *****', 

&  / , 3 (1X,G12 . 6) ) 

CALL  EXIT 

9  0  DEW  =  DMEM (1,1) 

C 

C  If  simulation  time  is  less  than  human  reaction  time  steering 
C  angle  set  to  zero 

IF (TTAB  .LT.  0.0)  THEN 
DEW  =  0 . ODO 
END  IF 
C 

C  Save  steer  and  time  values  for  next  calculation. 

C 

DFWLST  =  DEW 
TLAST  =  X 
TILAST  =  X 
C 

RETURN 

END 


c 

i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  ic  ic  ic  it  ic  it  ic  it  ic  ic  ic  it  ic 
^k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  k  if  if  if  k  if  if  if  if  if  if  if  k 

c 

C  ***  Transition  Matrix  Calculation  Subroutine  *** 

C 

C 

C  TRANS:  Computes  transition  matrix  (and  integral)  of  the  linearized 


C  system,  F,  described  in  references.  Result  stored  in  common 

C  arrays  TTT  and  TTTl  respectively.  10  pts  per  preview  interval. 

C 

C - Author  and  Modification  Section - 

C 


C  Author :  C .  C .  MacAdam 
C 

C  Date  written:  01/01/88 
C 
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C  Written  on: 

C 

C  Modifications: 

C 

C - 

C - Algorithm  Description - 

C 

C  Purpose  and  use:  Used  by  the  driver  model  in  predicting  future  states 
C 

C  Error  conditions: 

C 

C  Machine  dependencies:  none 
C 

C  Called  by:  DRIVGO 
C 

C - 

C 

SUBROUTINE  TRANS 
C 

C - Variable  Descriptions - 

C 

C - Arguments  passed:  None 

C 

INTEGER  R,  W 

double  precision  SV ( 4 ) , SD ( 4 ) , SVI ( 4 ) , GRAV, TICYCL , TSS , DMAX, XP, YP, 

&  TAUMEM , TEE ,RM,A,B,RI,PSIO, TLAST , DFWLAST , TILAST , 

&  DMEM , XT , YT , CAE , CAR , WHBS , WE , WR , U , TTT , TTTl , G , 

&  AAA, BBB , CCC , DDD , RATIO , DEBT , A1 , B1 , A2 , B2 , Cl , C2 , 

&  ULAST,TIME 

C 

C - COMMON  blocks - 

C 

COMMON  /DRVSTl/  GRAV,  TICYCL,  TSS,  DMAX,  XP(IOO),  YP(IOO),  TAUMEM, 

1  TEE,  RM,  A,  B,  RI ,  PS 10,  NTF,  NP,  TLAST,  DFWLAST,  TILAST, 

2  DMEM(1000,2) ,  XT(IOO),  YT(IOO) 

SAVE /DRVSTl/ 

COMMON  /DRIV/  CAE,  CAR,  WHBS,  WE,  WR,  U 
SAVE/DRIV/ 

COMMON  /INOUT/  R,  W 
SAVE/INOUT/ 

COMMON  /TRSSTR/  TTT (4, 4, 10),  TTTl ( 4 , 4 , 10 ) ,  G(4) 

SAVE/TRSSTR/ 


Control  Coefficients  A,  B,  C,  D  defined  in  section  5.5  of 
C  reference  (1)  and  passed  from  DADS  through  common  block  VEHTYP 

C  (  A  =  1,  C=l;  B  =  D  =  k  =  0  defines  a  conventional  front  steer 

C  vehicle ,  etc . ) 

C 

COMMON  /VEHTYP/  AAA,  BBB,  CCC,  DDD,  RATIO 
SAVE /VEHTYP/ 

C 

C - COMMON  Variables - 

C 

C  R . Driver  Model  Input  I/O  unit  ( " DMINPUT . INP " ) 

C  W . Driver  Model  Output  I/O  unit  ( "DMOUTPUT.OUT" ) 

C 

C - DRIV.BLK  common  block  variables - 
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C  CAF... total  cornering  stiffness  of  tires  on  left  front  susp  (Ib/rad) 

C  CAR... total  cornering  stiffness  of  tires  on  left  rear  susp  (Ib/rad) 

C  WHBS .. wheelbase  of  vehicle  (center-line  of  front  &  rear  susp  (ft) 

C  WF.... static  load  on  front  suspension  (lb) 

C  WR.... static  load  on  rear  suspension  (lb) 

C  U . initial  velocity  (ft/sec) 

C 

C - DRVSTl.BLK  common  block  variables - 

C 

C  GRAV . gravitational  constant 

C  TICYCL ...  driver  model  sample  time  (sec) 

C  TSS . minimum  preview  time  (sec) 

C  DMAX . upper  bound  on  front  wheel  angle  steer  (rad) 

C  XP,YP....x-y  path  coords(SAE)  wrt  inertial  coords  (input)  (ft) 

C  TAUMEM. .. driver  transport  time  dealy  (input  parameter)  (sec) 

C  TFF . driver  model  preview  time  (input  parameter)  (sec) 

C  RM . vehicle  mass  (slug) 

C  A . distance  from  c.g.  to  front  suspension  center-line  (ft) 

C  B . distance  from  c.g.  to  rear  suspension  center-line  (ft) 

C  RI . total  vehicle  yaw  inertia  (slug-ft) 

C  PSIO . current  yaw  angle  reference  value  (rad) 

C  NTF . number  of  points  in  the  preview  time  interval 

C  NP . number  of  points  in  the  x-y  trajectory  table 

C  TLAST . . . . last  time  driver  model  calculated  a  steer  value  (sec) 

C  DFWLAST . . last  value  of  steer  calculated  by  driver  model  (rad) 

C  TILAST . . . last  sample  time  driver  model  calculated  a  steer  value  (sec) 

C  DMEM . 2 -dim  array  (time  &  steer  history)  used  in  delay  calculat'n 

C  XT , YT ....  transformation  of  XP,YP  in  vehicle  body  axes  (ft) 

C 

C - TRSSTR.BLK  common  block  variables - 

C 

C  TTT . . . .transition  of  matrix  at  10  discrete  points  in  preview  interval 
C  TTTl . . .integral  of  trans  matrix  wrt  preview  time 

C  G . vector  of  control  gain  coefficients 

C 

C - VEHTYP  common  block  variables - 

C 

C  AAA.  ...  Control  coefficient  A  defined  in  section  5.5  of  ref  (1) 

C  BBB ....  Control  coefficient  B  defined  in  section  5.5  of  ref  (1) 

C  CCC ....  Control  coefficient  C  defined  in  section  5.5  of  ref  (1) 

C  DDD ....  Control  coefficient  D  defined  in  section  5.5  of  ref  (1) 

C  RATIO.. rear  steer  /  front  steer  ratio,  k,  defined  in  section  5.5 
C 

C - Local  variables - 

C 

C  DEBT . time  step  in  local  Euler  integration  (sec) 

C  A1 . lat  accel  coefficient  of  sideslip  veloc  in  linearized  system 

C  B1 .  "  yaw  rate 

C  A2 . yaw  accel  "  sideslip  vel  " 

C  B2 .  "  yaw  rate 

C  Cl . steer  control  gain  coefficient  for  lateral  accel 

C  C2 . steer  control  gain  coefficient  for  yaw  moment 

C  ULAST . . . . last  value  of  forward  velocity  (ft/sec) 

C  NBEG . integer  startin  counter  value 

C  NENDl ....  integer  ending  counter  value 

C  NENDV. . . .integer  ending  counter  value 
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C  J . integer  counter 

C  SV . state  vector:  Y,v,r,Yaw,x  (SAE) 

C  SVl . integral  of  state  vector 

C  SD . state  vector  derivative 

C 

C - Functions  and  subroutines - 

C 

C  None 

C 

C - 

C 

C - Process  Block - 

C 

C 


DEBT  =  0.0 IDO 

A1  =  -2.  *  (CAE  +  CAR)  /  RM  /  U 

B1  =  2.  *  (CAR  *  B  -  CAE  *A)  /RM/U-U 

A2  =  2.  *  (CAR  *  B  -  CAE  *  A)  /  RI  /  U 

B2  =  -2.  *  (CAR  *  B  *  B  +  CAE  *  A  *  A)  /  RI  /  U 

Cl  =  2.  *  (CAE  +  RATIO  *  CAR)  /  RM  *  AAA  +  BBB  /  RM 

C2  =  2.  *  (A  *  CAE  -  RATIO  *  B  *  CAR)  /  RI  *  CCC  +  DDD  /  RI 

ULAST  =  U 
G(l)  =  O.ODO 
G(2)  =  Cl 
G(3)  =  C2 
G(4)  =  O.ODO 
C 

DO  70  J  =  1,  4 
C 

NBEG  =  TSS  /  DEBT  +  1 

NENDl  =  (TEE  +  .001  -  TSS)  /  NTF  /  DEBT 
NENDV  =  NENDl 
DO  10  B  =  1,  4 

SV(B)  =  O.ODO 
SVI(B)  =  O.ODO 
10  CONTINUE 

TIME  =  0 . ODO 
C 

C  Initialize  each  state  in  turn  to  1.0  and  integrate. 


SV(J)  =  l.ODO 
DO  60  I  =  1,  NTF 

DO  40  K  =  NBEG,  NENDV 

SD(1)  =  SV(2)  +  U  *  SV(4) 

SD(2)  =  A1  *  SV(2)  +  B1  *  SV(3) 
SD(3)  =  A2  *  SV(2)  +  B2  *  SV(3) 

SD ( 4 )  =  SV ( 3 ) 

C 

DO  20  B  =  1,  4 

SV(B)  =  SV(B)  +  SD(B)  *  DEBT 
20  CONTINUE 

C 

TIME  =  TIME  +  DEBT 
C 

DO  30  B  =  1,  4 

SVI ( B )  =  SVI ( B )  +  SV ( B )  *  DEBT 
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30 


CONTINUE 


C 

40  CONTINUE 

C 

C  Store  "impulse"  responses  in  TTT  columns,  integral 
C  TTT  is  a  NPT-point  tabular  transition  matrix,  TTTl 
C  (See  references  2  &  3.) 

C 

DO  50  L  =  1,  4 

TTT (L,J,I)  =  SV(L) 

TTT1(L,J,I)  =  SVI(L) 

50  CONTINUE 


in  TTTl . 

is  its  integral. 


C 

NBEG  =  NBEG  +  NENDl 
NENDV  =  NENDV  +  NENDl 
C 

60  CONTINUE 


C 

70  CONTINUE 


RETURN 

END 


(~'-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k-k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 


c 


c 


c 


SUBROUTINE  Interf ace_Steering  (TIME , TDelta , loc_Yd, x_veh, y_veh, 
&  z_veh , Ps i , Theta , Phi , UX , UY , UZ , r , q , 

&  p , NW, ZLoadR, ZloadL , DEW) 

COMMON  /DRVSTl/  GRAY, TICYCL , TSS , DMAX , XP ( 10 0 ) , YP ( 10 0 ) , TAUMEM, 

1  TFF,RM, A,B,RI, PSIO,NTF,NP,TLAST,DFWLST,TILAST, 

2  DMEM(1000,2) ,XT(100) ,YT(100) 

COMMON  /DRIV/  CAE,  CAR,  WHBS ,  WE,  WR,  U 

COMMON/DMTIR/CCAFl , CCAF2 , CCARl , CCAR2 , FFZLl , FFZL2 , FFZL3 , FFZL4 , 

+  DMVELC 

SAVE/DRIVSTl/ 

SAVE /DRIV/ 

SAVE/ DMT IR/ 


double  precision 

& 

& 

& 

& 

& 

& 

& 

& 

& 


RD(200) ,TH(200) ,Y(5) , ZLoadR ( 2 ) , 
ZLoadL(2) , SCAN_RANGE ( 4 0 , 4 0 ) , 
SCAN_PHI (40), SCAN_THETA (40), 

TIME , TDelta , CAF , CAR , 

RM,RI, A,B, 

loc_yd,XV, YV, ZV, Psi, Theta, 
Phi,UX,UY,UZ,r,q,p,DFW, 

RMAX, SMAX_PHI , SMAX_THETA, 

SDELTA_PHI , SDELTA_THETA, THETRAD, 
DELTHRAD, PSIRAD , XSTAR , YSTAR , DFWNOW, 
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c 

c 

c 


& 

& 

& 

& 

& 

& 

& 


U,PI,GRAV,  TICYCL,  TSS,  DMAX,XP, 

YP , TAUMEM , TFF , RM , PS 10 , TLAST , DFWLST , 
TILAST , DMEM, XT , YT , WHBS , WF , WR , CCAFl , 
CCAF2 , CCARl , CCAR2 , FFZLl , FFZL2 , 

FFZL3 , FFZL4 , DMVELC , DPsi , X_Sen,  Y_Sen, 
Z_Sen , Ps i_Sen , Theta_Sen , Phi_Sen , 
x_veh , y_veh , z_veh , xs  tpr ev 


integer  NW,  kmax,  nuin_theta ,  k,  j  ,  npower ,  ntf  ,  np 
LOGICAL  FIRST, flag 


DATA  FIRST, flag  /. TRUE true . / 

DATA  PI  /3 . 1416/ 

C 

C  Assignments  below  necessary  to  avoid  using  dummy  arguments 
C  in  common  block  VEHCOORDS  . 

C 

xv=x_veh 

yv=y_veh 

zv=z_veh 

C 

WF  =  zloadr(l)  +  zloadl(l) 

WR  =  zloadr(2)  +  zloadl(2) 

WHBS  =  a+b 
CCAFl  =  caf/2.0 
CCAF2  =  caf/2.0 
CCARl  =  car/ 2.0 
CCAR2  =  car/ 2.0 
FFZLl  =  zloadl(l) 

FFZL2  =  zloadr(l) 

FFZL3  =  zloadl(2) 

FFZL4  =  zloadr(2) 

DMVELC  =  ux 
Dpsi  =  r 


IF (FIRST)  THEN 


CALL  Human_Driver  (NPOWER , RMAX , SMAX_PHI , 

1  SMAX_THETA , SDELTA_PHI , 

&  SDELTA_THETA, 

&  X_Sen,  Y_Sen,  Z_Sen, 

&  Psi_Sen, Theta_Sen, Phi_Sen) 


xstprev=xv 


END  IF 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

cccccc 

CCCCCC  Load  the  Y  vector  with  vehicle  states  from  DADS.  CCCCCCCCCCCCCCCCCC 
CCCCCC  CCCCCCCCCCCCCCCCCC 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c 


Yd) 

=  YV 

y(2) 

=  loc_yd 

y(3) 

=  r 

y(4) 

=  psi 

y(5) 

=  XV 

PSIRAD  =  Psi 

u  =  ux 


1 

1 

1 


CALL  Get_Obstacles  (FIRST,  TDelta,  SMAX_PHI ,  SMAX_THETA, 

RMAX , SDELTA_PHI ,  SDELTA_THETA , 

XV,  YV,  ZV,  Psi, Theta,  Phi, 
SCAN_RANGE , SCAN_PHI , SCAN_THETA) 


Find  the  minimum  range  in  the  set  of  vertical  scans.  Reduce  the 
3D  scan  to  a  2D  scan 


NUM_THETA  =  INT ( SMAX_THETA/ SDELTA_THETA) *2  +  1 
KMAX  =  INT (SMAX_PHI/SDELTA_PHI) *2  +  1 

DO  700  K  =  1,  KMAX 
RD(K)  =  RMAX 
TH(K)  =  SCAN_PHI(K) 

DO  800  J  =  1,  NUM_THETA 

IF(SCAN_RANGE(J,K)  . LE .  RD(K))  THEN 
RD(K)  =  SCAN_RANGE(J,K) 

END  IF 

800  CONTINUE 

700  CONTINUE 

Interfacing  with  the  original  obstacle  avoidance  subroutine 

THETRAD  =  SMAX_PHI 
DELTHRAD  =  SDELTA_PHI 
PSIRAD  =  Psi 


CALL  Avoid_Obstacles  (THETRAD,  DELTHRAD,  NPOWER,  XV,  YV,  ZV, 
1  PSIRAD,  KMAX,  RD,  TH,  XSTAR,  YSTAR) 


Human  steering  model 

CALL  HUMAN_STEERING (TIME, Tdelta, Y, DFW, DFWNOW, 
&  XSTAR, YSTAR) 

IF (FIRST)  THEN 

WRITE (7 , 705) 

705  FORMAT ("  TIME,  XV,  YV, " , 

1"  UX  UY  Psi", 
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DFW, 


XSTAR , " , 


1"  DPsi, 

1 "  YSTAR " ) 

END  IF 

FIRST  =  .FALSE. 


return 

END 


itiriritiriririfitirirififiririfitiririfiriririfiriririfiriririfiririritiriririf-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k'k'k'k'k'k'k-k'k'k'k-k'k'k'k-k 


Subroutine  CALCRS  calculates  RSTAR,  the  average  value  of  the  range  array. 

SUBROUTINE  CALCRS (  UMAX , R , TH , THMAX , DELTH , RSTAR ) 

- VARIABLE  DESCRIPTIONS - 

- ARGUEMENTS  PASSED - 

UMAX  -  Number  of  sensor  increments 
R  -  Range  array  from  GENRAY 

TH  -  Theta  array  from  GENRAY 
THMAX  -  Sensor  half  field  of  view 
DELTH  -  Sensor  field  of  view  increment 
RSTAR  -  Previewed  range 


double  precision  R (*), TH (*), THMAX, DELTH, RSTAR, 

&  SUM 

C 

SUM  =0.0 
C 

DO  100  I=1,IMAX 

SUM  =  SUM  +  R(I) 

100  CONTINUE 
C 

RSTAR  =  SUM/IMAX 
C 

RETURN 

END 

C 

icitiriritifiririfitirififitirififitiriritirirififitirififitiririfitiriritif-k'k-k-k-k'k-k-k-k-k-k-k-k'k-k-k-k'k-k-k-k'k'k'k-k'k'k'k-k'k'k'k-k'k'k 


68 


o  o  n  o  o 


C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


Subroutine  CALCTH  calculates  THSTAR,  the  WEIGHTED  average  of  the 
range- theta  array. 

SUBROUTINE  CALCTH (  UMAX , R , TH , THMAX , DELTH , NPOWER , THSTAR ) 
- VARIABLE  DESCRIPTIONS - 


- ARGUEMENTS  PASSED - 

UMAX  -  Number  of  sensor  angle  increments 
R  -  Range  array  from  GENRAY 

TH  -  Theta  array  from  GENRAY 

THMAX  -  Sensor  half  field  of  view 
DELTH  -  Sensor  field  of  view  increment 
NPOWER  -  Sensor  power 

THSTAR  -  Weighted  average  of  the  range-theta  array 


double  precision  R (*), TH (*), THMAX, DELTH, THSTAR, 
&  SUMl , SUM2 

SUMl  =0.0 
SUM2  =0.0 


C 

C 


DO  100  I=1,IMAX 

SUMl  =  SUMl  +  (R(I) **NPOWER) *TH(I) 
SUM2  +  (R(I) **NPOWER) 


SUM2 

100  CONTINUE 


THSTAR  =  SUM1/SUM2 

RETURN 

END 


iciriririfiririfititirififitirifititifirifirififititirififiririfititiririfirir-k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-k-k-k-k-k'k'k'k'k'k'k'k'kic'k'k'k'k'k'k 


Subroutine  CHECKRTH  recalculates  RSTAR  if  RSTAR  is  greater  than  the 
range  to  an  obstacle's  edge. 

SUBROUTINE  CHECKRTH ( IMAX , R , TH , RSTAR , THSTAR ) 


C 

C - VARIABLE  DESCRIPTIONS 

C 

C - ARGUEMENTS  PASSED - 

C 
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C  IMAX  -  Number  of  sensor  angle  increments 

C  R  -  Range  array  from  GENRAY 

C  TH  -  Theta  array  from  GENRAY 

C  THSTAR  -  Sensor  angle  increment  corresponding  to  RSTAR 

C  RSTAR  -  Maximum  range  within  the  sensor  field  of  view 

C - 

C 

C 

double  precision  R (200 ), TH (200 ), THSTAR, 

&  A, RANGE, RSTAR 

C 

A=RSTAR 

From  the  range-theta  profile  find  the  range  at  THSTAR 
DO  100  I=1,IMAX-1 

IF ((THSTAR  .GE.  TH ( I ) )  .AND.  (THSTAR  . LE .  TH ( I+l ) ) ) THEN 
RANGE=R(I) + ( (R(I+1) -R ( I ) ) / (TH(I+1) -TH ( I ) ) ) * (THSTAR-TH ( I ) ) 
END  IF 
100  CONTINUE 


IF  RSTAR  is  greater  than  the  range  from  the  range-theta  profile 
reduce  RSTAR  by  20% 


IF (A  .GE.  RANGE) THEN 
A=0 . 80*RANGE 
END  IF 

RSTAR=A 

RETURN 

END 


irififiririrififiritifirifirifitiritififiririfitifitififirirififiritifiriririfif-k-k'k-k-k-k'k-k-k-k'k-k-k-k'k-k-k-kic'k'k-k'k'k'k-kic'k'k-k'k'k'k 


Algorithm  Descript ion================================: 

C  Purpose  and  use: 

C  This  routine  calculates  terms  associated  with  the  user  defined 
C  algebraic  element. 

C 

C  Error  conditions:  none 

C 

C  Machine  dependencies:  none 
C 

0=================================================================: 


SUBROUTINE  FR3512  (  TIME,  ENS ,  FN35,  IC ,  RC ,  NEL,  lEVAL,  IMODUL, 
&  IBLOCK,  IFN,  ERRCOD,  INFOF,  UPDATI ,  SMPSTP, 
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A,  lA,  TOL  ) 


C============Variable  Descript ions====================================: 

C 

C - Arguments  passed - 

C 

C  TIME . Current  simulation  time. 

C  ENS . Vector  of  control  variable  values. 

C  FN35 . Vector  used  if  there  are  algebraically  related  nodes. 

C  IC . Array  of  control  element  integer  data. 

C  RC . Array  of  control  element  real  data. 

C  NEL . Number  of  user  algebraic  elements  in  the  model. 

C  lEVAL . Analysis  status  flag  for  controls  (=1...6). 

C  IMODUL . Control  module  currently  being  processed  (=12)  . 

C  IBLOCK . Number  of  the  user  algebraic  element  currently  being 

C  processed.  (=1...NEL) 

C  IFN . Used  if  the  control  system  has  algebraic  loops  in  the 

C  path. 

C  ERRCOD . Error  code  (zero  represents  no  error)  . 

C  INFOF . File  unit  for  the  information  file. 

C  UPDATI .  From  Rev7  on  this  flag  is  no  longer  used.  User  should 

C  ignore  this  flag. 

C  SMPSTP .  From  Rev7  on  this  flag  is  no  longer  used.  User  should 

C  ignore  this  flag. 

C  A . Array  of  all  real  (double  precision)  data  used  in  the 

C  analysis. 

C  lA . Array  of  all  integer  data  used  in  the  analysis. 

C  TOL . If  time  -  sample  time  is  within  this  tolerance  then 

C  this  is  a  sample  step. 

C 

INTEGER  NEL,  IC(NEL,*),  lEVAL,  IMODUL,  IBLOCK,  IFN,  ERRCOD, 

&  INFOF ,  I A ( 0 : 1 ) ,  GETNOD ,  INDXAR 


DOUBLE  PRECISION  ENS ( * ) ,  FN35(*),  RC(NEL,*),  TIME,  A(0:1) 
LOGICAL  UPDATI ,  SMPSTP 
external  GETNOD,  INDXAR 
C 

C - COMMON  blocks - 

common  /vehcoords/  x_veh, y_veh, z_veh, xO , yO , zO 

save/vehcoords/ 

save/vehspecs/ 

C 

C - Local  variables - 


C 

C  INODn . Input  value  of  the  n ' th  input  node. 

C  OUTNOD. .. .Value  of  the  output  node  after  calculations. 

C  IVALx . Integer  values  passed  in  from  the  preprocessor  for 

C  general  use. 

C  VALx . Real  values  similar  to  IVALx 

C 

DOUBLE  PRECISION  T,  INODl ,  IN0D2 ,  IN0D3 , IN0D4 ,  IN0D5 , 

&  IN0D6,  IN0D7,  IN0D8 ,  OUTNOD,  SRATE, 

&  SMPLOW,  SMPHGH,  TLSMP,  TSMP,  VALl ,  VAL2 , 

&  VAL3 ,  TOL , xd, yd, zd, zw, zf r ( 2 ) , zf 1 ( 2 ) , roll , 

&  pitch, yaw, crstf r ( 2 ) , crstf 1 ( 2 ) , mass 5 , Izz5 , 

&  cgf s , cgrs , pubts , dadsts , cgX, cgY, cgZ , strcom. 
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c 

c- 

c 

c 

c- 

c 


&  f stang , vehwid, x, y , z , vmass , xw, yw, ezero , 

&  eone , etwo , ethree , TDelta , oldtim, ydLoc , 

&  drvtrq, rfwang, rfww, rrww , x_veh , y_veh , z_veh, 

&  xO , yO , zO , xprev 

INTEGER  IVALl,  IVAL2 ,  IVAL3 ,  numwhl , numrb , numsen , pubrat , i 

logical  first, flag 


-Functions  and  subroutines- 
none 


-DATA  statements - 

data  vmass , first , oldtim, flag  /  0 . 0 ,. true ., 0 . 0 ,. true .  / 


C============Process  Block: 

T  =  TIME 


OUTNOD 

= 

0 . 0 

INODl 

= 

ENS (IC (IBLOCK, 

1)  ) 

IN0D2 

= 

ENS (IC (IBLOCK, 

2)  ) 

IN0D3 

= 

ENS (IC (IBLOCK, 

3)  ) 

IN0D4 

= 

ENS (IC (IBLOCK, 

4)  ) 

IN0D5 

= 

ENS (IC (IBLOCK, 

5)  ) 

IN0D6 

= 

ENS (IC (IBLOCK, 

6)  ) 

IN0D7 

= 

ENS (IC (IBLOCK, 

7)  ) 

IN0D8 

= 

ENS (IC (IBLOCK, 

8)  ) 

IVALl 

= 

IC (IBLOCK, 

10) 

IVAL2 

= 

IC (IBLOCK, 

11) 

IVAL3 

= 

IC (IBLOCK, 

12) 

VALl 

= 

RC (IBLOCK, 

1) 

VAL2  = 

RC (IBLOCK, 

2) 

VAL3  = 

RC (IBLOCK, 

3) 

IF  (  lEVAL  .EQ.  1  : 

)  THEN 

''ifififififififififififififif-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 

The  user  should  place  the  desired  calculations  below  this  comment.  * 
Note  that  the  final  output  node  calculation  should  be  placed  in  * 

OUTNOD.  For  instance,  if  the  output  node  was  to  be  the  cubic  root  * 
of  the  first  input  node,  the  calculation  would  be  as  follows:  * 

OUTNOD  =  IN0Dl**(l/3)  * 

Leave  the  rest  of  the  code  alone.  * 

* 

It  is  possible  that  this  element  maybe  digital.  Digital  means  * 
that  the  output  is  delayed  by  srate.  If  the  element  is  digital  ’ 
then  IC ( IBLOCK, 13 )  is  one  and  the  following  code  is  executed.  * 

If  the  element  is  not  digital  then  this  code  is  ignored.  Note  ’ 

the  output  node  must  be  calculated  before  this  block  of  code.  * 


O’* 

O’* 

O’* 

O’* 

O’* 

O’* 

c* 
c* 
c* 
c* 
c* 
c* 

i(  i(  i(  i(  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  i(  if  i(  i(  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  -k  ic  ic  ic  it  ic  ic  ic  it  ic  ic  ic  it  ic 

c 

c 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
ccccccc  cccc 

CCCCCCC  Grab  model  states  (nodes)  and  prepare  to  pass  them  out  CCCC 
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ccccccc  cccc 
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
c 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


CCCCCCC 

ccccccc 

DADS  outputs  units  in  the  following  format  : 

CCCCCCC 

ccccccc 

CCCCCCC 

ccccccc 

Length 

inch 

CCCCCCC 

ccccccc 

Mass 

(lb*sec**2) /inch 

CCCCCCC 

ccccccc 

Force 

lb  (pound) 

CCCCCCC 

ccccccc 

ccccccc 

ccccccc 

The  following  vehicle  state  units  will  be  changed 

ccccccc 

ccccccc 

to  represent 

ccccccc 

ccccccc 

ccccccc 

ccccccc 

Length 

in  foot 

ccccccc 

ccccccc 

Mass 

in  ( lb*sec* *2 ) / f oot 

ccccccc 

ccccccc 

Force 

remains  lb  (pound) 

ccccccc 

ccccccc 

ccccccc 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c 

if (first) then 

numwhl  =  indxar ( ' a ' , 3 , 31, 0 , 42 , ia) 

numrb  =  indxar ( 'a' ,3,3,0,l,ia) 

crstfr(l)  =  a ( indxar ( ' a ' , 3 , 3 1 , 1 , 8 , ia) ) 

crstfr(2)  =  a ( indxar ( ' a ' , 3 , 3 1 , 3 , 8 , ia) ) 

crstfl(l)  =  a ( indxar ( ' a ' , 3 , 3 1 , 2 , 8 , ia) ) 

crstfl(2)  =  a ( indxar ( ' a ' , 3 , 3 1 , 4 , 8 , ia) ) 

massS  =  (a ( indxar ( ' a ' , 3 , 3 , 1 , 1 , ia) ) ) *12 . 0 

xO  =  (ens (getnod( 'CHASSIS_X_POS  ')))/12.0 

yO  =  (ens (getnod( 'CHASSIS_Y_POS  ')))/12.0 

zO  =  (ens (getnod( 'CHASSIS_Z_POS  ')))/12.0 

xprev=xO 

C  Temporary  hardcode  for  HMMWV  Izz5  (slug*ft),  mass  (slugs),  cgfs  (ft)and 

cgrs  (ft) 

C  while  node  value  is  not  setting. 

C 

C  Izz5  =  (a ( indxar ( ' a ' , 3 , 3 , 1 , 4 , ia) ) ) / 12 . 0 

Izz5  =  7500.0 

C  cgfs  =  (ens (getnod( ' VEHCG_FRSUSP_DIST  ')))/12.0 

cgfs  =  6.8 

C  cgrs  =  (ens (getnod( ' VEHCG_RRSUSP_DIST  ')))/12.0 

cgrs  =  4.0 

C  vehwid  =  (ens (getnod( ' VEHICLE_WIDTH  ' ) ) ) /12 . 0 

do  555  i = 1 , numrb 

vmass  =  vmass  +  (a ( indxar ( ' a ' , 3 , 3 , i , 1 , ia) ) ) *12 . 0 
555  continue 

first  =  .false, 
endif 

C 

x=  (ens (getnod( 'CHASSIS_X_POS  ')))/12.0 

y=  (ens (getnod( 'CHASSIS_Y_POS  ')))/12.0 

z  =  (ens (getnod( 'CHASSIS_Z_POS  ')))/12.0 

x_veh=x 

y_veh=y 

z_veh=z 

xd  =  (ens (getnod( 'CHASSIS_X_VEL  ')))/12.0 

yd  =  (ens (getnod( 'CHASSIS_Y_VEL  ')))/12.0 
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c 

c 

c 


zd  =  (ens (getnod( 'CHASSIS_Z_VEL  ')))/12.0 

xw  =  ens (getnod( 'CHASSIS_ROLL_RATE  ')) 
yw  =  ens (getnod( 'CHASSIS_PITCH_RATE  ')) 
zw  =  ens (getnod( 'CHASSIS_YAW_RATE  ')) 
roll  =  ens (getnod( 'CHASSIS_ROLL_ANG  ')) 

pitch  =  ens (getnod( 'CHASSIS_PITCH_ANG  ')) 
yaw  =  ens (getnod( ' CHASSIS_YAW_ANG  ' ) ) 

zfr(l)  =  a ( indxar ( ' a ' , 3 , 3 1 , 1 , 42 , ia) ) 
zfr(2)  =  a ( indxar ( ' a ' , 3 , 3 1 , 3 , 42 , ia) ) 
zfl(l)  =  a ( indxar ( ' a ' , 3 , 3 1 , 2 , 42 , ia) ) 
zfl(2)  =  a ( indxar ( ' a ' , 3 , 3 1 , 4 , 42 , ia) ) 
fstang  =  ens (getnod ( ' FR_STEER_ACT_SENSOR  ' 
rfwang  =  ens (getnod (' FR_WHEEL_WRT_CHASSIS ' 
ydLoc  =  (ens (getnod ( 'CHASSIS_LOCAL_Y_VEL 
rfww  =  ens ( getnod ( ' RF_WHEEL_RATE  ' ) ) 

rrww  =  ens ( getnod ( ' RR_WHEEL_RATE  ' ) ) 


)  ) 

)  ) 

'  ) ) ) /12.0 


if ( t-oldtim. le . 0 . 0 ) then 
TDelta  =  0.001 
else 

TDelta  =  t-oldtim 
endif 


if ( iblock . eq. 1 ) then 

call  VEH_STEER ( t , crstf 1 , crstf r , zf 1 , zf r ,  vmass , cgf s , cgrs  ,  Izz5  , 
&  vehwid, y , yd, zw, yw, xw, roll , pitch, yaw, x, 

&  fstang , rfwang , xd, z , zd, TDelta , ydLoc , numwhl , 

&  strcom) 

outnod  =  strcom 
el seif ( iblock . eq. 2 ) then 
call  speed (rrww, drvtrq) 
outnod  =  drvtrq 
outnod  =  0.0 
endif 

oldtim  =  t 

--Following  code  only  executed  if  element  is  digital. 

IF (IC (IBLOCK, 13 )  .EQ.  1 ) THEN 
SRATE  =  RC (IBLOCK, 4) 

TLSMP  =  RC (IBLOCK, 5) 

TSMP  =  TLSMP  +  SRATE 
SMPLOW  =  TSMP  -  TOL 
SMPHGH  =  TSMP  +  TOL 

IF(T  .GE.  SMPLOW  .AND.  T  . LE .  SMPHGH) THEN 
RC ( IBLOCK , 6 )  =  OUTNOD 
ELSE 

OUTNOD  =  RC ( IBLOCK , 6 ) 

ENDIF 

ENDIF 
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if  i(  if  i(  i(  if  i(  i(  if  i(  i(  i(  i(  if  i(  i(  i(  i(  if  i(  if  i(  i(  if  i(  i<  -k  -k  -k  -k  -k  -k  -k  -k  ic  it  ic  it  ic  it  ic  ic  ic  ic  ic  ic  ic 


^  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if 

C*  This  should  be  the  end  of  the  user  code.  The  final  output  node  * 

C*  calculation  should  have  been  placed  in  the  variable  OUTNOD.  * 

^  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if  if 

ENS ( IC ( IBLOCK , 9 ) )  =  OUTNOD 

ELSE  IF  (  lEVAL  . EQ .  2  )  THEN 

WRITE  (  INFOF,  100  ) 

ERRCOD  =  23512 

100  FORMAT (/ , 'USRALG  elements  not  allowed  in  algebraic  loop.',/  ) 

END  IF 

RETURN 

END 

ifififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififif 

ifififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififif 


C  ***  Matrix  Product  Subroutine  *** 

C 

C  GMPRD:  Computes  matrix  product 
C 

C - Author  and  Modification  Section 

C 

C  Author:  IBM  Scientific  Subroutine 


C 

C  Date  written: 

C 

C  Written  on: 

C 

C  Modifications:  C.  MacAdam 
C 

C - 

C 

C - Algorithm  Description 

C 

C  Purpose  and  use:  R  =  A  B 
C 

C  Error  conditions: 

C 

C  Machine  dependencies:  none 
C 

C  Called  by:  DRIVER 
C 
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SUBROUTINE  GMPRD(A,  B,  R,  N,  M,  L) 


C 

C - Variable  Descriptions - 

C 

C - Arguments  passed: 

C 

C  A . N  X  M  matrix 

C  B . M  X  L  matrix 

C  R . N  X  L  resultant  matrix  =  A  B  product 

C  N . integer  row  dimension  of  A 

C  M . integer  column  dimension  of  A  (or  row  dimension  of  B) 

C  L . integer  column  dimension  of  B 

C 


double  precision  A (N*M) , B (M*L) , R (N*L) 

C 

C - COMMON  blocks - 

C 

C  None 

C 

C - COMMON  Variables - 

C 

C  None 

C 

C - Local  variables - 

C 

C  IR,  IK,  M,  K,  L,  IR,  JI,  J,  N,  IB,  IK,  etc . integer  counters 

C 

C - Functions  and  subroutines - 

C 

C  None 

C 

C - 

C 

C - Process  Block - 

C 

C 


IR 

=  0 

IK 

=  -M 

DO 

10  K 

=  1, 

L 

IK 

=  IK 

+ 

M 

DO 

10  J 

= 

1, 

N 

IR 

= 

IR 

+ 

1 

JI 

= 

J  - 

-  N 

IB 

= 

IK 

R(IR) 

= 

0  . 

DO 

10 

I 

= 

1, 

M 

JI 

= 

JI 

+ 

N 

IB 

= 

IB 

+ 

1 

10  R(IR)  =  R(IR)  +  A(JI)  *  B(IB) 
RETURN 
END 
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C  $Id:  inain.f,v  1.3. 2. 1.2.1  1997/12/23  15:55:12  bill  Exp  $ 


C  MAIN:  Entry  point  for  DADS-3D  -  used  to  reset  array  sizes. 


C============Author  and  Modification  Section^ 

C 

C  Author :  Chuck  Mead 

C 


C  Date  written:  January  29,  1986 
C 

C  Written  on:  MicroVAX  II 

C 

C  Modifications: 

C  1)  1/28/88  Changed  the  /AIA/  common  block  from  named  common  to 
C  blank  common  to  avoid  any  potential  problems  resul- 

C  ting  from  different  sizes  of  the  same  common  block 

C  in  different  routines.  (Chuck  Mead) 

C  2)  4/5/88  Increased  A  and  lA  to  700000  and  150000.  (Dick  Kading) 

C  $Log:  main.f,v  $ 

C  Revision  1.3. 2. 1.2.1  1997/12/23  15:55:12  bill 

C  Added  a  comment  for  NSAVE13 . 

C 

c  Revision  1.3. 2.1  1996/07/26  16:33:50  alan 

c  Replaced  the  explicit  definition  of  the  A  and  lA  arrays  with  the  common 
block 

c  ' dadsaia . blk ' .  This  common  block  is  already  being  used  by  several  routines 
c  (particularly  in  the  MatLab  and  MatrixX  interface  routines),  and  Chuck  says 
c  it's  OK . 
c 

c  Revision  1.3  1995/10/23  17:39:17  bill 

c  Define  number  of  data  points  stored  for  continuous  delay  element  here, 
c  This  allows  user  to  increase  the  size, 
c 

c  Revision  1.2  1995/03/31  14:54:41  chuck 

c  The  big  7.6  commit, 
c 

c  Revision  1.1.1.1.10.1  1995/03/23  18:09:28  chuck 

c  Changed  A  and  lA  arrays  from  blank  common  to  the  named  common  blocks 
c  used  with  the  DADS/Plant  interfaces, 
c 

C  Copyright  (c)  CADSI  1988-1995 
C 

0======================================================================= 

C============Algorithm  Descript ion====================================== 

C 

C  Purpose  and  use: 

C  This  subroutine  is  the  entry  point  for  the  DADS-3D  analysis  pro- 

C  gram.  The  sizes  of  the  two  main  arrays,  A  and  lA,  are  defined 

C  here.  To  change  the  size  of  either  the  integer  array  lA  or  the 

C  double  precision  array  A,  change  the  value  of  the  associated 

C  parameter,  i.e.,  PIASIZ  for  lA  or  PASIZ  for  A.  Then,  recompile 

C  this  routine,  replace  it  in  the  MOD3D  object  library,  and  relink 
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c 

c 

c 

c 

c 

c 

C: 


DADS-3D. 

Error  conditions:  none 

Machine  dependencies:  none 


SUBROUTINE  MAINA 


C 
C 
C 
C 
C 
C 
C 

C 

C  DADSAIA:  Common  blocks  for  A  and  lA  arrays 
C 

C - Main  real  &  integer  data  arrays 

C 

C  A . Vector  for  all  real  data  in  the  system. 

C  lA . Vector  for  all  integer  data  in  the  system. 

C  PASIZ. . . .Parameter  defining  the  upper  bound  of  the  A  array. 

C  PIASIZ ...  Parameter  defining  the  upper  bound  of  the  lA  array. 

C  PLASIZ ...  Parameter  used  to  size  A  array  for  DADS/plant  runs. 

C  PLIASIZ ...  Parameter  used  to  size  lA  array  for  DADS/plant  runs. 
C 

C  Copyright  (c)  CADSI  1996 

0=================================================================== 


============Variable  Descriptions 

- Arguments  passed - 

none 

- COMMON  blocks - 


INTEGER  PASIZ,  PIASIZ 
INTEGER  PLASIZ,  PLIASIZ 

PARAMETER  (  PASIZ=4000000 ,  PIASIZ=800000  ) 

INTEGER  IA(0: PIASIZ) 

DOUBLE  PREC I S ION  A ( 0 : PAS I Z ) 

COMMON  /DADS I A/  I A 
COMMON  /DADS A  /  A 

COMMON  /PLDADSA  /  PLASIZ,  PLIASIZ 

SAVE  /DADSIA/,  /DADSA/ ,  /PLDADSA/ 

C 

INTEGER  NSAVE13 

COMMON/C8CONDLY/NSAVE13 

SAVE/C8C0NDLY/ 

C 


78 


o  o 


C - Local  variables - 

C 

C  ASIZE. . . .Local  variable  for  upper  bound  array  declaration  for  the 
C  array  A. 

C  lASIZE ...  Local  variable  for  upper  bound  array  declaration  for  the 
C  array  lA. 

C  NSAVE13 .. Maximum  number  of  points  saved  in  the  control  analaog  delay 

C  element.  The  user  may  increase  this  if  needed. 

C 

INTEGER  ASIZE,  lASIZE 
C 

C - Functions  and  subroutines - 

C 

EXTERNAL  MAINE 


C============Process  Block====================================: 

C - Set  the  value  of  the  two  array  sizes  equal  to  the  declared 

C  parameters . 


ASIZE  =  PASIZ 
lASIZE  =  PIASIZ 

C--Number  of  data  points  in  continuos  delay  element. 
NSAVE13  =  5000 

C - Continue  the  analysis. 

CALL  MAINE  (  A,  lA,  ASIZE,  lASIZE  ) 


RETURN 

END 

C 


subroutine  SPEED (ww, drvtrq  ) 

C 

double  precision  gain, ww, spderr, drvtrq 
C 

gain  =  10000. ODO 
spderr  =  60.0  -  ww 
C 

drvtrq  =  spderr*gain 
C 

if (drvtrq. le. -20000.0) then 
drvtrq  =  -20000.0 
el seif (drvtrq. ge . 40000 . 0 ) then 
drvtrq  =  40000.0 
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endif 


C 

return 

end 

C 


c 

C  Subroutine  TRANXY  transforms  polar  RSTAR  and  THSTAR  to  inertial  XSTAR 
C  and  YSTAR 
C 


C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 


SUBROUTINE  TRANXY  (X, Y, PS I , RSTAR, THSTAR, XSTAR, YSTAR) 
- VARIABLE  DESCRIPTIONS - 


-ARGUEMENTS  PASSED- 


X  -  Vehicle's  forward  displacement,  (ft) 

Y  -  Vehicle's  lateral  displacement,  (ft) 

PSI  -  Vehicle's  yaw  angle,  (rad) 

RSTAR  -  Average  of  the  range  array  or  previewed  range 
THSTAR  -  Weighted  average  of  the  theat  array  or  previewed  angle 
XSTAR  -  Previewed  forward  location  (ft) 

YSTAR  -  Previewed  lateral  location  (ft) 


DOUBLE  PRECISION  X , Y , PSI , RSTAR , THSTAR , XSTAR , YSTAR 

XSTAR=X+RSTAR*COS (PSI+THSTAR) 

YSTAR= Y+RSTAR  *  S IN ( PS I +THSTAR ) 


RETURN 

END 


c 

subroutine  VEH_STEER ( t , crstf 1 , crstf r , zf 1 , zf r , vmass , cgf s , cgrs , 
&  Izz5 , vehwid, y , yd, zw, yw, xw, roll , pitch, 

&  yaw, X, f stang , rfwang , xd, z , zd, TDelta, 

&  ydLoc , numwhl , strcom) 

C 


C 


C 


COMMON 

1 

2 

COMMON 


/DRVSTl/  GRAV,TICYCL,TSS,DMAX,XP(100) ,YP(100) ,TAUMEM, 
TEE , RM , A , B , RI , PSIO , NTF , NP , TLAST , DFWLST , TILAST , 
DMEM(1000,2) ,  XT(IOO),  YT(IOO) 

/DRIV/  CAE,  CAR,  WHBS ,  WE,  WR,  U 


SAVE/DRIVSTl/ 
SAVE /DRIV/ 


double  precision  t , TDelta , strcom, newang , DEW, oldang , stgain, 

&  f stang , xd, xdglim, crstf 1 ( 2 ) , crstf r (2 ) , zf 1 (2 ) , 

&  zf r ( 2 ) , vmass , cgf s , cgrs , Izz5 , y , yd, zw, yaw, x. 
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&  vehwgt , vehwid, strlim, pi , z , roll , pitch, yw, xw, 

&  ydLoc , caf , car , grav, ticycl , tss , dmax, xp, yp, 

&  taumem, tf f , rm, a,b,ri,psio,tlast, dfwlst , tilast , 

&  dmem, xt , yt , whbs , wf , wr , u , zd, rfwang 

integer  numwhl , numaxl , ntf , np 
C 

data  stgain, xdglim, DFW, strlim, pi  /50000. 0,20. 0,0. 0,34. 0,3. 14159/ 

C 

C 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


c  c 

C  Strcom  (steer  command)  is  a  torque  in  inch*lbs,  stgain  (steer  gain)  C 
C  is  in  inch*lbs/radian,  xd  (  x  direction  velocity)  is  in  C 
C  inches/sec,  xdglim  (x  direction  velocity  gain  limit)  is  in  C 
C  inches/sec,  newang  (updated  average  steer  angle)  is  in  radians,  C 
C  and  oldang  (previous  average  steer  angle)  is  in  radians.  C 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c 

oldang  =  fstang 
vehwgt  =  vmass*32.2 
numaxl  =  numwhl/ 2 
caf  =  crstf 1 ( 1 ) +crstf r ( 1 ) 
car  =  crstf 1 ( 2 ) +crstf r ( 2 ) 
rm  =  vmass 
ri  =  Izz5 
a  =  cgfs 
b  =  cgrs 
C 

i f ( xd . ge . 3 . 0 ) then 

call  Interf ace_Steering  ( t , TDelta , ydLoc , x, y, z , yaw, pitch, 

&  roll , xd, yd, zd, zw,  yw, xw, numaxl , 

&  zf r , zf 1 , DFW) 

endif 
C 
C 

newang  =  DFW 

strcom  =  -1 . 0 *( stgain* (newang  -  rfwang)) 

C 

return 

end 

**************************  OF  FORTRAN  CODE  **************************** 
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NO.  OF 
COPIES 


1 


1 


1 


3 


1 


1 


3 


1 


ORGANIZATION  NO.  OF 

COPIES  ORGANIZATION 


ADMINISTRATOR 

DEFENSE  TECHNICAL  INFO  CTR 

ATTN  DTICOCA 

8725  JOHN  J  KINGMAN  RD  STE  0944 
FTBELVOIR  VA  22060-6218 
*pdf  file  only 

DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  Cl  IS  R  REC  MGMT 
2800  POWDER  MILL  RD 
ADELPHIMD  20783-1197 

DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  Cl  OK  TECH  LIB 
2800  POWDER  MILL  RD 
ADELPHIMD  20783-1197 

DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  D  D  SMITH 
2800  POWDER  MILL  RD 
ADELPHIMD  20783-1197 

DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  SE  RM  E  BURKE 
G  GOLDMAN 

AMSRD  ARE  SE  DC  A  GOLDBERG 
2800  POWDER  MILL  RD 
ADELPHIMD  20783-1197 

DOD  JOINT  CHIEFS  OF  STAFF 
ATTN  J39  CAPABILITIES  DIV 
CPT  J  M  BROWNELL 
THE  PENTAGON  RM  2C865 
WASHINGTON  DC  20301 

DIRECTOR  CIA 
ATTN  D  MOORE 
WASHINGTON  DC  20505-0001 

PM  ABRAMS  TANK  SYSTEM 
ATTN  SFAEGCSAB  COL  KOTCHMAN 
PLEITHEISER  H  PETERSON 
WARREN  MI  48397-5000 

PM  M1A2 

ATTN  SFAEGCSAB  ETC  R  LOVETT 
WARREN  MI  48397-5000 


1  PMMlAl 

ATTN  SFAEGCSAB  ETC  L  C  MILLER  JR 
WARREN  MI  48397-5000 

1  PEO-GCS 

BRADLEY  FIGHTING  VEHICLES 
ATTN  MKING 
WARREN  MI  48397-5000 

1  PM  BFVS 

ATTN  ATZB  BV  COL  C  BETEK 
FORT  BENNING  GA  31905 

1  PM  M2/M3  BFVS 

ATTN  SFAE  GCS  BV  ETC  J  MCGUINESS 
WARREN  MI  48397-5000 

3  PM  BCT 

ATTN  SFAE  GCS  BCT  COL  R  D  OGG  JR 
JGERLACH  TDEAN 
WARREN  MI  48397-5000 

1  PM  lAV 

ATTN  SFAE  GCS  BCT  ETC  J  PARKER 
WARREN  MI  48397-5000 

1  PM  NIGHT  VISION/RSTA 

ATTN  SFAEIEW&SNV  COL  BOWMAN 
10221  BURBECKRD 
FT  BELVOIR  VA  22060-5806 

1  NIGHT  VISION  &  ELEC  SENSORS  DIR 
ATTN  DR  A  F  MILTON 

10221  BURBECK  RD  SUITE  430 
FT  BELVOIR  VA  22060-5806 

2  CDR  US  ARMY  TRADOC 
ATTNATINZA  R  REUSS 

ATINI  C  GREEN 
BLDG  133 

FT  MONROE  VA  23651 

1  OFC  OF  THE  SECY  OF  DEFENSE 
CTR  FOR  COUNTERMEASURES 
ATTN  MASCHUCK 
WSMR  NM  88002-5519 

1  US  SOCOM 

ATTN  SOIOJAF  J  GOODE 

7701  TAMPA  POINT  BLVD  BLDG  501 

MCDILL  AFB  FL  33621-5323 
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1  CDR  US  ARMY  ARMOR  CTR  &  FT  KNOX 

TSM/ABRAMS 
ATTN  COL  D  SZYDLOSKI 
FT  KNOX  KY  40 12 1 

1  CDR  US  AMBL 

ATTN  COLJJUGHES 
FT  KNOX  KY  40 12 1 

1  DIR  OF  COMBAT  DEVELOPMENT 

ATTN  ATZK  FD  W  MEINSHAUSEN 
BLDG  1002  ROOM  326 
1  ST  CAVALRY  DIV  RD 
FT  KNOX  KY  40I2I-9I42 

I  COMMANDING  OFFICER 

MARINE  CORPS  INTEL  ACTIVITY 
ATTN  COL  WILLIAM  BARTH 
3300  RUSSELL  ROAD  SUITE  250 
QUANTICOVA  22I34-50II 

4  CDR  US  TACOM-ARDEC 

ATTN  AMSTA  AR  TD  M  DEVINE 
M  FISETTE 

AMSTA  ARFSAM  J  FENECK 
AMSTA  AR  FSA  P  D  PASCUA 
PICATINNY  ARSENAL  NJ  07806-5000 

4  CDR  US  TACOM-ARDEC 

ATTN  AMSTA  AR  FSA  S  R  KOPMANN 
H  KERWIEN  K  JONES 
A  FRANCHINO 

PICATINNY  ARSENAL  NJ  07806-5000 

4  CDR  US  TACOM-ARDEC 

ATTN  AMSTA  AR  FSA  T  A  LAGASCA 
AMSTA  AR  FSP  D  LADD 
M  CILLI  M  BORTAK 
PICATINNY  ARSENAL  NJ  07806-5000 

3  CDR  US  TACOM-ARDEC 

ATTN  AMSTA  AR  FSP  G  A  PEZZANO 
RSHORR 

AMSTA  AR  FSP  I  R  COLLETT 
PICATINNY  ARSENAL  NJ  07806-5000 

7  CDR  US  TACOM-ARDEC 

ATTN  AMSTA  AR  CCH  A  M  PALTHINGAL 
A  VELLA  E  LOGSDON 
R  CARR  M  MICOLICH 
M  YOUNG  A  MOLINA 
PICATINNY  ARSENAL  NJ  07806-5000 


3  CDR  US  TACOM-ARDEC 

ATTN  AMSTA  AR  QAC  R  SCHUBERT 

AMSTA  AR  WE  C  R  FONG  S  TANG 
PICATINNY  ARSENAL  NJ  07806-5000 

I  SAIC 

ATTN  K  A  JAMISON 
PO  BOX  4216 

FT  WALTON  BEACH  FL  32549 

4  PEO-GCS 

ATTN  SFAEGCS  C  GAGNON 

SFAEGCSW  APUZZUOLI 
SFAE  GCS  BV  J  PHILLIPS 
SFAE  GCS  LAV  COL  T  LYTLE 
WARREN  MI  48397-5000 

4  PEO-GCS 

ATTN  SFAE  GCS  AB  SW  DR  PATTISON 
SFAE  GCS  AB  LF  ETC  PAULSON 
SFAE  GCS  LAV  M  T  KLER 
SFAE  GCS  LAV  FCS  MR  ASOKLIS 
WARREN  MI  48397-5000 

3  CDR  US  ARMY  TACOM 

ATTN  AMSTA  TR  DR  R  MCCLELLAND 
MR  BAGWELL 
AMSTA  TA  J  CHAPIN 
WARREN  MI  48397-5000 

1 2  CDR  US  ARMY  TACOM 

ATTN  AMSTA  TR  R  DR  J  PARKS  C  ACIR 

S  SCHEHR  D  THOMAS  J  SOLTESZ 
SCAITO  KLIM  JREVELLO 
B  BEAUDOIN  B  RATHGEB 
MCHAIT  SBARSHAW 
WARREN  MI  48397-5000 

8  CDR  US  ARMY  TACOM 

ATTN  AMSTA  CM  XSF  RDRITLEIN 

MR  HENDERSON  MR  HUTCHINSON 
MR  SCHWARZ  S  PATHAK 
R  HALLE  J  ARKAS  G  SIMON 
WARREN  MI  48397-5000 

5  PEO  PM  MORTAR  SYSTEMS 

ATTN  SFAE  AMO  CAS  IFM  L  BICKLEY 
M  SERBAN  K  SLIVOVSKY 
SFAE  GCS  TMA  R  KOWALSKI 
SFAE  GCS  TMA  PA  E  KOPACZ 
PICATINNY  ARSENAL  NJ  07860-5000 
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3  MIT  LINCOLN  LABORATORY 
ATTN  J  HERD  G  TITI  D  ENGREN 
244  WOOD  STREET 
LEXINGTON  MA  02420-9108 

2  THE  UNIV  OF  TEXAS  AT  AUSTIN 
INST  FOR  ADVANCED  TECH 
ATTN  IMCNAB  S  BLESS 
PO  BOX  20797 
AUSTIN  TX  78720-2797 

1  INNOVATIVE  SURVIVABILITY  TECH 
ATTN  J STEEN 
PO  BOX  1989 
GOLETACA  93 1 16 

1  SUNY  BUFFALO 

ELECTRICAL  ENGINEERING  DEPT 
ATTN  JSARJEANT 
PO  BOX  601900 
BUFFALO  NY  14260-1900 

1  GENERAL  DYNAMICS  LAND  SYSTEMS 
ATTN  D  GERSDORFF 
PO  BOX  2074 
WARREN  MI  49090-2074 

1  CDR  US  ARMY  CECOM 
ATTN  WDEVILBISS 
BLDG  600 

FT  MONMOUTH  NJ  07703-5206 

1  MARCORSYSCOM/CBG 
ATTN  CPT  J  DOUGLAS 
QUANTICOVA  22134-5010 

2  CDR  USAIC 

ATTN  ATZB  CDF  MAJ  J  LANE 
D  HANCOCK 
FTBENNINGGA  31905 

2  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  SL  EA  R  CUNDIFF 

AMSRD  ARE  SL  EM  J  THOMPSON 
WSMR  NM  88001-5513 

4  UNITED  DEFENSE  ADV  DEV  CTR 
ATTN  K  GROVES  J  FAUL  T  WINANT 

V  HORVATICH 
328  BROKAW  ROAD 
SANTA  CLARA  CA  95050 


2  NORTHROP  GRUMMAN  CORP 

ATTN  A  SHREKENHAMER  D  EWART 
1100  W  HOLLYVALE  STREET 
AAUSACA  91702 

I  CDR  US  ARMY  AMCOM 

ATTN  AMSAM  RD  ST  WF  D  LOVELACE 
REDSTONE  ARSENAL  AL  35898-5247 

I  OFC  OF  THE  SECY  OF  DEFENSE 
ATTN  ODDRE  (R&T)  G  SINGLEY 
THE  PENTAGON 
WASHINGTON  DC  20301-3080 

I  US  MILITARY  ACADEMY 

MATH  SCIENCES  CTR  OF  EXCELLENCE 
DEPT  OF  MATHEMATICAL  SCIENCES 
ATTN  MDN  A  MAJ  HUBER 
THAYER  HALL 
WEST  POINT  NY  10996-1786 

1  DIR  US  ARMY  WATERWAYS  EXPER  STN 

ATTN  RAHLVIN 
3909  HALLS  FERRY  ROAD 
VICKSBURG  MS  39180-6199 

1  NATL  INST  STAN  AND  TECH 
ATTN  K  MURPHY 
100  BUREAU  DRIVE 
GAITHERSBURG  MD  20899 

1  CDR  US  ARMY  MMBL 
ATTN  MAJ  J  BURNS 
BLDG  2021 

BLACKHORSE  REGIMENT  DRIVE 
FT  KNOX  KY  40 12 1 

2  DIRECTOR 

NASA  JET  PROPULSION  LAB 
ATTNLMATHIES  K  OWENS 
4800  OAK  GROVE  DRIVE 
PASADENA  C A  91 109 

I  DIRECTOR 

AMCOM  MRDEC 

ATTN  AMSMI  RD  W  C  MCCORKLE 
REDSTONE  ARSENAL  AL  35898-5240 

I  COMMANDER 
CECOM 

SP  &  TERRESTRIAL  COM  DIV 
ATTN  AMSEL  RD  ST  MC  M  H  SOICHER 
FT  MONMOUTH  NJ  07703-5203 


85 


NO.  OF 

COPIES  ORGANIZATION 


NO.  OF 

COPIES  ORGANIZATION 


1  COMMANDER 

US  ARMY  INFO  SYS  ENGRG  CMD 
ATTN  ASQB  OTD  F  JENIA 
FTHUACHUCAAZ  85613-5300 

1  COMMANDER 

US  ARMY  NATICK  RDEC 
ACTING  TECHNICAL  DIR 
ATTN  SSCNC  T  P  BRANDLER 
NATICK  MA  01760-5002 

1  COMMANDER 

ARMY  RESEARCH  OFC 

4300  S  MIAMI  BLVD 

RSCH  TRIANGLE  PARK  NC  27709 

1  COMMANDER 

US  ARMY  STRICOM 
ATTN  J  STAHL 
12350  RSCH  PARKWAY 
ORLANDO  FL  32826-3726 

1  COMMANDER 

US  ARMY  TRADOC 

BATTLE  LAB  INTEGRATION  7  TECH  DIR 
ATTN  ATCD  B  J  A  KLEVECZ 
FT  MONROE  VA  23651-5850 

1  COMMANDER 

ATTN  CODE  B07  J  PENNELLA 
17320  DAHLGREN  ROAD 
BLDG  1470  RM  1101 
DAHLGREN  VA  22448-5100 

1  DARPA 

3701  N  FAIRFAX  DRIVE 
ARLINGTON  VA  22203-1714 

1  COMMANDER 

US  ARMY  AVIATION  &  MISSILE  CMD 
ATTN  AMSAM-RD-SS-EG  AKISSELL 
BLDG  5400 

REDSTONE  ARSENAL  AL  35898 

1  OFC  OF  THE  PROJECT  MGR 

MANEUVER  AMMUNITION  SYSTEMS 
ATTN  SBARRIERES 
BLDG  354 

PICATINNY  ARSENAL  NJ  07806-5000 

1  COMMANDER 

US  ARMY  TRADOC  ANALYSIS  CTR 
ATTN  ATRC-WBA  J  GALLOWAY 
WSMRNM  88002-5502 


1  FASTTRACK  TECH  INC 
ATTN  JK  GARRETT 
540  CEDAR  DRIVE 
RADCLIFFKY  40160 

1  DIR  USARMY  TACOM 
6501  E  ELEVEN  MILE  RD 
WARREN  MI  48397-5000 

ABERDEEN  PROVING  GROUND 

2  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  Cl  OK  (TECH  LIB) 
BLDG  305  APGAA 

1  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  HR  SC 
BLDG  459 

1  CDR  US  ARMY  EDGEWOOD  RDEC 
ATTN  SCBRD  TD  J  VERVIER 
APG  EA 

2  CDR  US  ARMY  TECOM 
ATTN  AMSTE  CD  B  SIMMONS 
AMSTE  CD  M  R  COZBY 
RYAN  BLDG 

4  DIR  US  AMSAA 

ATTN  AMXSY  D  M  MCCARTHY 
P  TOPPER 

AMXSY  CA  G  DRAKE  S  FRANKLIN 
BLDG  367 

7  CDR  US  ATC 

ATTNCSTEAEC  COL  ELLIS 
CSTE  AEC  TD  J  FASIG 
CSTE  AEC  TE  H  CUNNINGHAM 
CSTE  AEC  RMC  A  MOORE 
CSTE  AEC  TE  F  P  OXENBERG 
A  SCRAMLIN 
CSTE  AEC  CCE  W  P  CRISE 
BLDG  400 

1  PM  ODS 

ATTN  SFAE  CBD  COL  B  WELCH 
BLDG  4475 
APG  EA 
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5  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  WM  J  SMITH 
E  SCHMIDT  B  RINGER 
T  ROSENBERGER 
B  BURNS 
BLDG  4600 

3  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  WM 
C  SHOEMAKER 
J  BORNSTEIN 

AMSRD  ARE  WMBF  JWALL 
BLDG  II2I 

2  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  WM  B  A  HORST 
W  CIEPIELLA 
BLDG  4600 

3  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  WM  BA  D  LYONS 

AMSRD  ARE  WM  BC  P  PLOSTINS 
AMSRD  ARE  WM  BD  B  FORCH 
BLDG  4600 

2  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  WM  MB  L  BURTON 
BLDG  4600 

7  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  WM  BF  T  HAUG 
P  FAZIO  R  PEARSON 
M  FIELDS  GHAAS 
WOBERLE  JWALD 
BLDG  390 

7  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  WM  TE  A  NIILER 
G  THOMSON  TKOTTKE 
MMCNEIR  PBERNING 
J  POWELL  C  HUMMER 
BLDG  120 

1  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  WM  TC  R  COATES 
BLDG  309 


NO.  OF 

COPIES  ORGANIZATION 
I  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  SL  BG  M  ENDERLEIN 
BLDG  247 

I  DIRECTOR 

US  ARMY  RSCH  LABORATORY 
ATTN  AMSRD  ARE  SL  EM  C  GARRETT 
BLDG  390A 
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